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CHAPTER  1 : 
Introduction 


For  nearly  a  decade,  the  Naval  Postgraduate  School  (NPS),  Case  Western  University  and  ad¬ 
ditional  collaborators  have  been  involved  in  a  program  to  develop  an  autonomous  surf-zone 
robot.  In  general,  unmanned  systems  provide  significant  advantages  for  military  operations  and 
commercial  applications.  Autonomous  and  remotely  operated  systems  provide  advanced  ca¬ 
pabilities  at  a  lower  cost,  while  not  placing  personnel  in  harm’s  way.  There  have  been  recent 
advancements  in  unmanned  systems  that  operate  solely  in  one  mode,  such  as  land  based  or  wa¬ 
ter  based.  Overcoming  the  transition  from  one  mode  to  another,  such  as  an  amphibious  robot, 
still  requires  significant  research.  The  environmental  conditions  and  duality  associated  with  the 
surf-zone  presents  a  unique  set  of  challenges.  There  have  been  several  previous  platforms  that 
have  attempted  to  address  the  difficulties  associated  with  these  harsh  conditions.  Some  of  these 
platforms  have  provided  invaluable  insight  into  complex  mobility  and  autonomy. 

Interest  for  these  types  of  systems  that  operate  in  coastal  areas  and  shallow  beaches  is  widespread 
Platforms  wielding  these  capabilities  can  be  outfitted  with  a  multitude  of  sensors  to  accomplish 
a  spectrum  of  missions.  Tasking  may  include  minesweeping  or  clearance,  terrain  or  bathymetry 
surveys,  covert  reconnaissance  and  surveillance.  As  sensor  packages  become  more  compact,  it 
is  possible  to  envision  equipping  such  a  robot  with  a  chemical  detection  unit  that  may  be  able  to 
search  for  specific  compounds  or  chemical  weapons.  The  versatility  of  these  platforms  justifies 
the  need  to  research  and  develop  a  robust  surf-zone  robot. 

1.1  Background 

1.1.1  Previous  Designs 

Whegs™  (wheel-legs)  describes  a  class  of  robot  that  characterizes  its  locomotion  based  on  a 
fusion  of  a  wheel  design  with  crawling  leg  that  was  inspired  by  the  motion  of  biological  organ¬ 
isms.  Case  Western  Reserve  University’s  Biologically  Inspired  Robotics  Laboratory  developed 
this  means  of  locomotion  under  Roger  Quinn  [6].  The  idea  was  based  on  the  high  maneuver¬ 
ability  of  a  cockroach  and  its  ability  to  overcome  adverse  obstacles.  This  design  concept  has 
been  incorporated  into  several  versions  of  surf-zone  robots. 

An  earlier  Whegs™  design  was  the  Dayton  Area  Graduate  Studies  Institute  (DAGSI)  Whegs™ 
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Figure  1 .1 :  From  [1],  a  picture  of  Agbot 


prototype  called  Agbot.  NPS  and  Case  Western  Reserve  University  collaborated  to  build  Agbot, 
pictured  in  Figure  1.1.  Agbot’s  design  consists  of  six  Whegs™,  a  single  section  body,  and  is 
steered  by  angling  the  front  Whegs™,  similar  to  an  automobile.  The  autonomy  was  limited  to 
waypoint  navigation  from  a  control  station.  Agbot’s  main  purpose  was  to  act  as  a  test  platform 
for  the  mobility  of  the  Whegs™  and  their  ability  to  overcome  obstacles.  Detailed  information 
regarding  Agbot  is  available  in  [1]. 

Variants  of  Agbot  have  been  produced,  which  have  consistent  designs  and  been  subject  to  ad¬ 
ditional  testing  [2].  Although  success  has  been  shown  for  climbing  large  obstacles,  improved 
designs  have  the  potential  for  significant  advances.  One  proposed  improvement  is  replacing  the 
rear  segment  of  the  main  body  with  an  autonomous  tail.  The  main  motivation  for  incorporating 
a  tail  into  a  surf-zone  robot  is  to  climb  larger  obstacles  and  terrain  [2].  This  modeling  was  done 
in  Working  Model  2D  and  still  needed  to  be  verified  with  prototype  trials.  The  addition  of  a  tail 
and  removal  of  two  Whegs™  also  implied  that  the  stability  would  be  improved  (discussed  in 
Section  2.1.1)  with  a  new  design  comprising  four  legs  per  Wheg™  instead  of  three.  A  concep¬ 
tual  rendering  of  a  next  generation  robot  is  seen  in  Figure  1.2.  For  a  more  detailed  explanation 
of  these  mobility  concepts  see  Section  2.1. 

ROBSTER,  Figure  1.3,  was  developed  to  serve  as  a  test  platform  for  the  addition  of  a  tail  [3]. 
This  initial  investigation  into  tail  control  was  conducted  by  Courtney  Holland  at  NPS  in  June 
2009.  ROBSTER’s  design  incorporated  the  new  Whegs™  style  and  consisted  of  a  rigid  tail  that 
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Figure  1 .2:  From  [2],  a  3D  rendering  of  a  future  design  called  Pelican  Whegs™ 


Figure  1 .3:  From  [3],  a  picture  of  Robster 


was  2/3  the  length  of  the  entire  robot.  The  torque  of  the  motor  and  gearing  allowed  the  tail  to  lift 
the  entire  rear  of  the  robot.  Dynamic  tests  were  conducted  to  determine  the  effects  of  using  the 
tail  for  climbing  assistance.  These  tests  did  not  create  a  high  centering  scenario  that  the  design 
is  susceptible  to.  Some  recommended  design  improvements  reported  by  Holland  included  the 
incorporation  of  a  solid-state  micro-electro-mechanical  systems  (MEMS)  inclinometer  and  an 
improved  control  algorithm  that  reduces  spurious  sensor  data. 
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Dunbar  developed  navigation  and  control  for  Agbot  in  his  thesis,  including  an  effective  way- 
point  navigation  algorithm  that  interfaced  with  a  Java  based  graphical  user  interface  (GUI), 
written  by  Uzun,  for  a  robot  named  Bender  [2].  Agbot  could  navigate  up  to  10  waypoints,  and 
report  current  GPS  position  and  heading.  Williamson  wrote  a  proportional,  integral,  deriva¬ 
tive  (PID)  control  scheme  to  control  the  motors  and  calibrated  the  appropriate  gains  for  an 
autonomous  ground  vehicle  named  Bigfoot  [7]. 

1.1.2  Related  Works 


Figure  1 .4:  From  [4],  a  3D  rendering  of  AQUA  outfitted  with  walking  legs 


There  are  a  variety  of  research  groups  that  have  studied  platforms  designed  for  autonomy  and 
some  use  specialized  methods  for  mobility.  Select  robots  have  also  been  designed  for  amphibi¬ 
ous  operations.  AQUA  is  an  advanced  robot  that  has  two  modes  of  operation,  the  land  based 
version  in  Figure  1.4  and  a  waterborne  variant  that  uses  flippers  instead  of  legs  [4].  The  vehi¬ 
cle’s  means  of  locomotion  are  unique  and  together  attempt  to  overcome  the  difficult  transition 
from  water  to  land.  The  Surf  Zone  Crawler  Group  from  the  Naval  Surface  Warfare  Center  has 
also  focused  on  this  operating  environment  in  order  to  provide  mine  detection  and  classification 
[8].  Their  group  proposed  that  different  unmanned  systems  work  as  a  team  to  perform  separate 
tasks.  Another  design  seen  in  Figure  1.5,  called  AmphiRobot,  uses  a  unique  tail  and  propeller 
system  to  provide  mobility  in  water  and  land  [5].  This  robot  features  a  variety  of  sensors  and 
servomotors  that  allow  for  object  avoidance  and  additional  autonomy.  Commercial  teams  have 
also  developed  crawling  robots  that  can  be  used  to  conduct  non-destructive  testing  inside  oil 


4 


and  chemical  tanks  [9].  In  order  to  deliver  accurate  inspection  results  these  adverse  constraints 
have  lead  to  interesting  designs  that  require  advanced  sensors  and  control  systems. 


Figure  1 .5:  From  [5],  a  3D  rendering  of  AmphiRobot 


Controls  and  robotic  operating  systems  are  active  areas  of  research  in  robotics.  Autonomously 
integrating  sensor  data  to  navigate  and  perform  tasks  in  an  unmanned  vehicle  is  a  big  ticket  item 
for  the  Defense  Department  and  the  private  sector.  Numerous  institutions  and  agencies,  such 
as  the  Defense  Advanced  Research  Projects  Agency  (DARPA)  and  (AUVSI)  have  sponsored 
many  competitions  and  research  projects  in  the  subject.  One  example  is  the  DARPA  Urban 
Challenge,  the  last  of  which  was  held  in  2007.  Teams  had  to  convert  automobiles  to  negotiate  a 
complex  course  in  an  urban  environment  autonomously  with  no  human  assistance  [10]. 

The  operating  systems  and  architectural  structures  for  controlling  robots  are  many  and  varied. 
Previous  work  was  done  using  DynamicC©.  Developed  for  use  with  Rabbit  Microprocessors, 
Dynamic  C©provides  multitasking  capability  for  a  robotic  project.  Lopez,  Bigfoot,  and  Agbot 
programs  were  all  written  in  Dynamic  C.  The  Robotic  Operating  System  (ROS)  is  another 
example  of  an  object  oriented  operating  system,  which  is  open  source  and  maintained  by  Willow 
Garage.  Programs  like  ROS  allow  for  rapid  prototyping  and  integration  of  software.  Examples 
include  the  AsTec  Quadrotor  unmanned  aerial  vehicle,  Clearpath  Kingfisher  sea-based  system, 
and  the  iRobot  Roomba  [11,  12]. 
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1.2  Concept  of  Operations 

An  autonomous  amphibious  vehicle  that  could  operate  in  the  surf-zone  would  provide  a  valuable 
capability  to  the  military.  An  inexpensive  robotic  platform  could  be  deployed  covertly  from  the 
sea  and  make  its  way  onto  the  shore  and  replace  the  need  to  send  in  a  human.  This  could  be 
accomplished  on  the  surface,  or  subsurface  depending  on  the  system. 

The  transition  from  the  sea  to  shore  is  the  unique  aspect  of  this  concept.  Crashing  waves,  rock 
formations,  and  other  features  provide  quite  the  challenge  in  negotiating  its  way  to  shore.  A 
surf-zone  robot  would  need  to  utilize  multiple  systems  to  successfully  make  this  transition. 

Once  ashore,  the  platform  can  perform  reconnaissance,  disable  mines,  or  deploy  devices  de¬ 
pending  on  the  mission  requirements.  This  is  only  a  short  list  of  the  capabilities  provided  by 
this  kind  of  vehicle.  The  sensor  and  mission  packages  could  be  modular  to  provide  flexibil¬ 
ity.  Multiple  sensor  inputs,  including  GPS,  inertial  navigation  systems  (INS)  and  stereovision, 
provide  positional  and  path-finding  capabilities. 

Communication  both  at  sea  and  ashore  will  need  to  be  handled  effectively.  Wireless  or  laser 
point-to-point  communications  will  work  well  for  the  surface.  Submerged  navigation  and  com¬ 
munications  can  be  handled  via  acoustic  beacons  like  Seaweb  [13].  The  concept  can  be  taken 
further  where  a  “mother  ship”  style  deployment  system  can  be  implemented.  An  offshore  plat¬ 
form  can  deploy  and  serve  as  the  communications  hub  for  the  surf-zone  robots.  Once  the  mis¬ 
sion  has  been  accomplished  the  robots  can  transition  from  shore  to  sea  for  scuttling  or  recovery. 
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CHAPTER  2: 

Concepts  for  Mobility  and  Navigation 


2.1  Mobility 

2.1.1  The  Use  of  Whegs™ 


Figure  2.1:  From  [1],  two  graphic  representations  of  Wheg™designs  where  left  is  four  spoke  variant  and  right  is 
three  spoke  variant 

The  Wheg™  has  been  an  instrumental  aspect  in  the  design  of  a  surf-zone  robot.  It  allows  for 
fast  travel  over  smooth  terrain  and  offers  the  ability  to  climb  over  obstacles.  Figure  2.1  shows 
a  basic  diagram  of  two  different  variations  of  Wheg™  design.  The  three  spoke  variant  was 
originally  introduced  on  a  robot  with  six  Whegs™.  Their  rotation  was  phased  in  a  manner  to 
always  have  three  Whegs™  in  contact  with  the  ground.  This  provided  sufficient  stability  during 
locomotion  and  while  standing  still.  The  three  spoke  variant  is  also  able  to  climb  a  higher  step 
size  when  compared  to  a  four  spoke  variant.  The  one  disadvantage  to  having  three  spokes  is 
that  the  center  of  rotation  has  a  large  vertical  variation  as  the  Wheg™  rotates  and  advances  the 
position  of  the  Wheg™.  This  produces  significant  vibrations  as  the  robot  moves  along  a  path. 
Having  groups  of  Whegs™  phased  together  reduces  the  overall  undulation  of  the  robot. 

The  four  spoke  variant  helps  limit  the  vibration  and  vertical  variation  seen  at  the  center  of  the 
Wheg™.  The  step  height  geometry  in  Figure  2.1  corresponds  to  a  30%  vertical  variation  for 
four  spokes  vice  50%  for  three  spokes.  More  spokes  offer  more  stability  since  there  is  one 
additional  leg  to  provide  support  through  one  rotation.  Ideally  a  complete  wheel  would  be  used 
to  limit  the  vibration,  however  that  would  sacrifice  the  ability  to  climb  obstacles  and  travel  in 
complex  terrain  such  as  loose  sand.  The  four  spoke  variation  does  limit  the  step  size  that  it  is 
able  to  climb  over;  however  it  is  only  a  six  percent  reduction.  Overall  the  Wheg™  has  proven 
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successful  for  surf-zone  operations  and  different  variants  can  be  used  for  specific  applications 

[1]. 


2.1.2  Optimizing  the  Center  of  Mass 


Figure  2.2:  From  [2]:  Left  image  shows  a  previous  generation  robot  successful  climbing  an  obstacle  in  modeling 
environment,  Center  picture  shows  accomplishing  the  task  by  a  prototype  robot,  Right  image  shows  a  theoretical 
design  with  a  tail  capable  of  climbing  a  higher  obstacle 


Previous  generations  of  Wheg™  robots  used  an  overall  symmetric  design  consisting  of  a  front 
and  rear  segment.  These  designs,  one  of  which  is  shown  in  Figure  2.2,  have  had  significant 
success  at  climbing  large  obstacles.  Replacing  the  rear  segment  of  the  articulating  body  with  a 
tail  shifts  the  center  of  mass  forward  on  the  robot  allowing  it  to  climb  20  precent  larger  obstacles 
[2].  Video  from  various  trials  was  carefully  reviewed  and  it  was  determined  that  the  location 
of  the  center  of  mass  relative  to  the  position  of  the  center  Wheg™  was  the  deciding  factor  for 
success.  The  tail  acts  as  a  support  point  behind  the  main  body  that  can  provide  leverage  to  lift  the 
rear  of  robot.  The  incorporation  of  the  tail  optimizes  the  center  of  mass  while  climbing  adverse 
terrain  and  increases  the  overall  mobility  of  a  surf-zone  robot.  Further  rigid  body  analysis  and 
modeling  should  be  investigated  to  determine  more  quantitative  insight  into  this  high  center 
scenario. 

2.2  Navigation 

2.2.1  Waypoint  Navigation 

Path  planning  using  GPS  waypoints  is  a  simple  but  effective  means  of  navigating  a  robot  through 
its  environment.  In  this  mode,  the  robot  will  find  the  heading  to  the  destination  and  drive  towards 
it.  Since  the  destination  is  set  by  the  user,  autonomy  in  this  mode  is  limited  to  driving  the  plant 
to  arrive  at  the  destination. 

For  this  method  of  implementation,  obstacle  avoidance  is  possible  only  through  the  user’s  se¬ 
lection  of  a  safe  path.  Implementation  of  object  avoidance  will  require  additional  sensor  input. 
Additional  sensors  can  be  used  to  implement  more  sophisticated  navigational  methods  such 
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Figure  2.3:  Example  of  PID  control  loop 


as  “bug”  algorithms  (that  skirt  around  the  edges  of  encountered  obstacles)  and  potential  field 
navigation  [14].  Advanced  path  planning  techniques  will  be  investigated  in  future  works. 

2.2.2  Plant  Control 

A  robot  using  a  compass  and  GPS  waypoints  needs  to  have  a  feedback  control  to  navigate 
successfully  to  its  destination.  Proportional,  Integral,  and  Derivative  feedback  control  (PID)  is 
a  popular  method.  PID  control  provides  effective  system  response  capabilities  and  tuning  of 
system  parameters  [15,  16]. 

Figure  2.3  illustrates  the  function  of  the  PID  feedback  loop  that  can  be  used  with  heading 
control.  First,  the  desired  heading  is  differenced  with  the  current  heading  and  the  error  signal 
is  produced.  This  error,  denoted  e(t),  is  then  fed  into  the  PID  compensator  which  consists  of  a 
series  of  gains  denoted  Kp,  A"*,  and  Kd,  such  that  the  control  signal  is  given  by: 

u(t)  =  Kp  e(t)  +  AT/  [  e(r)dr  +  Kn  (2.1) 

J-i  Jo 

Equation  2.1  shows  the  time  response  of  a  typical  PID  controller.  The  proportional  gain,  Kp, 
is  a  gain  that  is  applied  to  the  error  to  provide  a  signal.  This  gives  the  kick  to  the  plant  to  get 
it  traveling  towards  the  destination,  but  may  lead  to  “hunting.”  Hunting  is  where  the  robot’s 
heading  oscillates  about  the  desired  path.  This  typically  happens  when  the  gain  is  too  high 
and  is  known  as  under-damping.  If  Kp  is  too  low  the  response  will  take  a  long  time  to  reach 
the  ordered  heading  (i.e.  over-damped),  and  could  never  reach  the  waypoint  depending  on  the 
positional  geometry. 

Integral  control,  Kt,  helps  solve  some  of  these  issues.  The  integrator  sums  the  error  over  a 
given  period  of  time,  77,  and  then  applies  the  resulting  gain  to  the  plant  signal.  This  allows 
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the  feedback  loop  to  correct  for  encountered  friction  and  inertia  that  produces  a  response  offset. 
The  error  is  time-averaged,  and  will  give  a  boost  to  the  signal  if  the  outcome  is  slow  to  respond. 

The  final  component  is  the  derivative  control,  Kd,  which  helps  improve  dynamic  response. 
This  method  monitors  the  rate  of  change  in  the  error,  over  time  To,  which  can  help  minimize 
overshoot  of  the  desired  outcome. 

The  signal  generated  by  the  PID  control  is  then  fed  into  the  plant  which  produces  a  response. 
The  feedback  (current  heading  in  this  case)  is  then  fed  back  into  the  loop  and  the  process  starts 
again.  All,  or  some,  of  the  components  can  be  implemented  depending  on  the  desired  response 
required. 

2.2.3  Gain  Scheduling 

When  implementing  PID  control,  selecting  the  appropriate  gains  is  vital.  This  does  not  mean 
Kp,  Ki,  and  Kd  need  to  be  constant.  This  is  commonly  referred  to  as  gain  scheduling.  One  im¬ 
plementation  is  to  change  Kp,  Ku  and  Kd  based  off  of  the  current  terrain  the  robot  is  traversing. 
Another  use  is  to  limit  the  maximum  gain  until  a  threshold  is  reached.  For  example,  a  robot 
could  switch  off  PID  control  if  the  heading  error  was  greater  than  90  degrees.  If  outside  this 
tolerance,  the  robot  would  simply  turn  at  the  maximum  rate  until  the  error  dropped  below  the 
appropriate  level.  [16] 

A  more  detailed  discussion  of  PID  control  can  be  found  in  the  thesis  written  by  Dunbar  [1], 
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CHAPTER  3: 
Design 


The  first  step  in  creating  Mobility  Over  Non-trivial  Terrain  (MONTe)  is  to  dissect  the  concept 
of  operations  and  determine  characteristics  that  will  be  inherent  to  our  design.  For  example,  an 
amphibious  robot  could  be  designed  to  crawl  on  the  bottom  of  a  body  of  water,  float  at  the  sur¬ 
face,  or  possibly  be  engineered  to  do  both.  These  preliminary  design  constraints  establish  core 
capabilities  that  act  as  a  foundation  for  the  overall  design.  Our  team  placed  three  constraints  on 
the  MONTe.  First,  the  robot  is  watertight,  vice  free  flood,  and  is  positively  buoyant.  Designing 
the  robot  to  float  on  the  surface  improves  the  reliability  of  communications,  allows  for  the  re¬ 
ception  of  GPS  information,  and  simplifies  the  initial  testing  environment.  Secondly,  Whegs™ 
provide  MONTe’s  land  based  locomotion  and  a  tail  assists  in  climbing  terrain.  Lastly,  the  robot 
is  semi-autonomous  vice  tethered.  These  constraints  may  be  altered  for  later  builds  as  MONTe 
matures  through  testing  and  improvements. 

The  following  provides  a  quick  overview  of  the  design,  referencing  Figures  3.1  and  3.2.  For 
details  on  the  mechanical  design,  refer  to  Slatt  [17]. 


Left/Right 
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Figure  3.2:  Top  view  of  MONTe’s  components 


3.1  Mechanical  Components 

3.1.1  Main  Body 

The  challenge  of  making  MONTe  watertight  led  to  the  selection  of  a  Pelican  Case  for  the  main 
body.  This  allows  for  the  main  access  to  be  sealed  with  a  gasket  while  allowing  frequent  opening 
and  closing  throughout  testing.  As  additional  penetrations  are  made  through  the  case,  they  are 
sealed  individually  using  gaskets  or  techniques. 

3.1.2  Drive  Assembly 


Figure  3.3:  Picture  of  MONTe’s  drive  assembly  attached  to  half  of  a  radial  arm 


Attached  to  both  sides  of  the  main  body  are  the  left  and  right  drive  assemblies.  These  drive 
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assemblies  contain  the  drive  motor  shafts  that  connect  to  a  main  drive  belt.  It  also  houses  a 
suspension  system  for  both  the  front  and  rear  Whegs™.  Limit  switches  are  triggered  if  any 
force  displaces  this  suspension  from  its  equilibrium  position,  which  is  used  to  detect  a  free 
rotating  Wheg™.  The  drive  assembly  can  be  seen  in  Figure  3.3. 

3.1.3  Radial  Arm  Assembly 

The  radial  arm  assemblies  connect  each  Wheg™  to  its  associated  drive  assembly.  Each  assem¬ 
bly  contains  a  belt  and  pulley  configuration  that  transfers  torque  from  the  drive  assembly  to 
each  Wheg™.  It  comprises  two  halves  that  are  joined  together.  One  of  these  halves  can  be  seen 
in  Figure  3.3. 

3.1.4  Wheg™ 


Figure  3.4:  Picture  of  MONTe’s  initial  Wheg™design 


This  Wheg™  design,  Figure  3.4,  is  a  four  spoke  variant.  As  discussed  in  Section  2.1.1,  it 
offers  less  vibration  over  smooth  terrain,  but  limits  the  step  size  for  which  the  Wheg™  will 
climb.  There  has  also  been  a  decrease  in  the  total  number  of  Whegs™from  six  to  four.  The 
Whegs™  are  designed  to  be  easily  removed  and  allows  for  quick  implementation  of  changes  to 
the  Wheg™  design.  The  shape  and  symmetry  of  the  Wheg™  affect  its  ability  to  climb  and  will 
continue  to  be  adapted  throughout  the  development  process. 
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3.1.5  Water  Jets 


Figure  3.5:  Picture  of  MONTe’s  future  water  jet,  left  image  shows  the  placement  and  right  image  shows  the  compo¬ 
nents 

Water-borne  locomotion  will  be  provided  with  a  water  jet  system,  as  shown  in  Figure  3.5.  The 
motors,  ducting,  and  impeller  will  be  housed  in  the  main  body.  The  ducting  acts  as  a  new 
pressure  boundary  within  the  main  body.  This  allows  the  jet  motors  to  be  isolated  from  the 
water  while  providing  thrust. 

3.1.6  Tail 

One  of  the  major  advancements  of  MONTe  is  the  incorporation  of  an  autonomous  tail.  The 
tail  was  introduced  to  the  overall  design  in  order  to  overcome  a  susceptibility  to  high  centering 
by  moving  the  center  of  mass  forward.  The  tail  is  designed  to  assist  in  climbing  obstacles  and 
to  self-right  the  robot  in  the  event  it  becomes  flipped  over.  Tail  operation  is  modeled  for  to 
determine  performance,  which  will  be  discussed  in  the  Section  4.3. 

Mechanical  Design  of  Tail 

The  tail  for  MONTe  is  relatively  simple  in  construction  and  provides  the  mechanics  for  an  initial 
proof  of  concept.  The  most  apparent  simple  design  is  a  rigid  flap  that  attaches  to  the  robot  at 
a  joint  as  seen  conceptually  in  Figure  1.2.  Two  independent  joints  provide  rotational  motion 
along  the  same  axis.  To  prevent  the  tail  from  obscuring  any  sensors,  a  “wire  frame”  structure  is 
used,  Figure  3.6.  Additionally,  it  reduces  the  overall  weight  of  the  tail  and  maintains  a  similar 
level  of  strength. 

Steel  tube,  3/8”  outer  diameter  with  a  1/16”  wall  thickness  provides  the  main  structural  support 
to  create  the  tail.  When  in  a  stowed  position,  Figure  3.7,  the  tail  wraps  around  the  main  body 
case  of  MONTe,  as  in  Figure  3.6.  The  frame  is  supported  by  a  cross  member  that  bends  over  the 
top  of  the  case.  The  cross  member  is  placed  in  an  optimal  location.  It  is  positioned  close  to  the 
control  joints  to  provide  support  while  ensuring  enough  distance  to  prevent  interference  with 
components  when  in  the  extended  position.  The  tip  of  the  tail  is  left  hollow  to  allow  extensions 
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Figure  3.6:  A  picture  of  MONTe  with  the  tail  in  the  stowed  position 


Figure  3.7:  MONTe ’s  tail  positions  (a)  Down  (b)  Stowed  (c)  Neutral 


to  be  inserted  into  the  tubes.  These  extensions  allow  for  changing  the  overall  length  of  the  tail 
as  necessary  throughout  design  and  testing. 

The  tail  is  11.8  ounces  and  is  attached  to  a  high  torque  servo  drive  mechanism.  These  servo 
components  are  attached  to  the  drive  assemblies  using  steel  brackets  and  fasteners.  The  com¬ 
bined  bracket  and  servo  drive  mechanism  weighs  16.2  ounces.  Later  designs  will  reduce  the 
overall  weight  by  replacing  the  steel  bracket  with  lighter  weight  polycarbonate  materials.  Com- 
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bining  the  tail  bracket  with  the  drive  assembly  will  eliminate  fasteners  and  be  a  necessary  design 
improvement  to  maintain  watertight  integrity  of  the  overall  design.  This  single  unit  will  encase 
the  servo  drive  gears  shielding  it  from  debris  and  sand  and  thus  preventing  damage  to  the  gears. 


^  3\| 

✓  X 


Figure  3.8:  A  picture  of  the  high  torque  servo  drive  mechanism 

Tail  Drive  Mechanism  Design 

The  tail  drive  mechanism  consists  of  a  titanium  geared  hobby  servo,  HS-7955TG,  and  a  supple¬ 
mental  gearbox,  Figure  3.8.  The  stock  servos  were  modified  to  allow  continuous  rotation  of  the 
servo  and  incorporate  a  new  potentiometer  into  the  gear  train.  These  high  torque  servos  provide 
95-3300  oz-in  of  torque  [18]. 


Figure  3.9:  A  graphic  showing  a  simple  lever  arm  that  can  be  used  to  estimate  the  required  torque  output  of  the 
servo  drive  mechanism 


Torque  =  r  x  F  |Torque|  =  |r||F|  sin  6  =  Iw  (3.1) 
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Preliminary  calculations  were  necessary  to  provide  an  estimate  of  the  torque  required  to  operate 
the  tail.  The  general  torque  expression  is  given  by  Equation  3.1,  where  F  is  the  applied  force 
vector  and  r  is  the  displacement  vector  from  the  joint  to  the  applied  force.  Based  on  a  weight 
estimate  of  the  entire  robot  and  the  location  of  the  tail  joint,  that  equation  reduces  to  a  simple 
expression.  Figure  3.9  depicts  the  moment  arm  that  transmits  the  torque  of  the  center  of  mass 
on  the  tail  joint.  Based  on  an  anticipated  final  weight  (w)  of  20  lbs.  and  a  moment  arm  (/)  of  8 
in.,  the  estimated  total  torque  would  be  2600  oz-in.  Since  two  servo  mechanisms  will  be  used 
to  drive  the  tail,  each  unit  would  need  to  supply  roughly  1300  oz-in,  which  is  governed  by  the 
following  equations  [19]: 


Tm  =  Kt(j)Ia  (3.2) 

T  =  Jeq6  +  Feq6  (3.3) 

Jeq  ~  Ja  +  (3-4) 

Feq  ~  F a  +  ~TT^Fl  (3.5) 


As  seen  in  Equation  3.2,  torque  of  the  electric  tail  motor  is  proportional  to  the  current  that  it 
draws,  Ia,  flux  for  each  pole,  (j),  and  a  constant,  Kt,  related  to  the  physical  design  of  the  motor. 
This  is  a  design  issue  for  MONTe  since  current  is  provided  from  a  limited  battery  system, 
see  Section  3.4.  Even  if  ample  current  is  available,  electric  motors  will  stall  if  the  combined 
friction  and  inertia  applied  to  the  motor  are  too  great.  The  mechanical  equations  of  motion  for 
the  joint  are  given  by  Equation  3.3.  This  shows  that  torque  is  proportional  to  the  inertia,  Jeq,  and 
friction,  Feq,  through  either  the  angular  acceleration,  0,  or  angular  rate,  9.  When  considering  a 
joint  driven  by  an  electric  motor,  the  inertia  and  friction  can  be  divided  into  two  components,  the 
armature  of  the  motor,  a,  and  the  external  load,  L,  of  Equations  3.4  and  3.5.  When  considering 
MONTe’s  self-righting  high  torque  scenario,  the  load  produces  an  overwhelming  effect  over 
the  armature.  By  applying  a  mechanical  gear,  the  torque  is  transmitted  from  a  longer  moment 
arm  and  acts  to  reduce  the  inertial  and  frictional  effect  on  the  motor.  Equations  3.4  and  3.5  also 
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show  that  by  selecting  the  proper  gear  ratio,  N,  the  inertial  and  load  can  be  dominated  by  the 
armature  and  prevent  the  motor  from  stalling.  The  tradeoff  is  that  the  tail  rotation  speed  will 
be  reduced.  Testing  in  Section  4.3.1  further  investigates  the  torque  required  for  MONTe’s  servo 
drive  mechanism. 

3.2  Operating  System  Architecture 

Robots  designed  for  reconnaissance  must  be  capable  of  several  tasks  if  they  are  to  be  useful 
to  the  organization  that  operates  them.  A  robot  must  first  be  able  to  get  to  the  target  location. 
Once  there,  it  must  be  able  to  orient  itself  in  its  environment  and  the  region  of  interest.  Its 
sensor  package  must  be  able  to  find  and  record  data  of  interest.  Finally,  the  robot  must  be  able 
to  communicate  at  some  point  with  the  operator  in  order  to  complete  its  mission. 

The  level  of  autonomy  becomes  a  vital  part  the  design  implementation.  At  one  end  of  the  spec¬ 
trum  is  an  unmanned  vehicle  that  is  fully  user-operated.  While  easier  to  design  in  a  technical 
sense,  this  can  be  prohibitive  in  the  man-hours  required  to  operate  it.  Furthermore,  the  user 
will  most  likely  be  only  able  to  operate  one  unmanned  system  at  a  time.  The  other  end  of  the 
spectrum  is  a  system  designed  to  require  no  user  input  beyond  mission  parameters. 

MONTe  is  designed  to  operate  in  the  middle  of  the  spectrum.  To  be  semi-autonomous  in  nature, 
MONTe  can  communicate,  navigate  and  be  controlled  by  the  operator  as  the  situation  warrants. 

The  primary  operating  system  for  MONTe  is  located  on  the  LPC-100  computer.  The  main 
program’s  responsibilities  include  communications,  navigation,  plant  control  and  eventually 
stereovision.  The  architecture  of  the  operating  system  is  based  off  the  Robot  Operating  System 
(ROS). 

3.2.1  ROS  Overview 

ROS  is  an  open-source,  meta-operating  system  designed  for  robotic  applications.  It  uses  a  peer- 
to-peer  network  messaging  system  between  different  processes.  The  different  processes  are 
loosely  coupled  by  being  compartmentalized.  It  is  not  a  real  time  operating  system.  ROS  is  also 
designed  to  allow  for  portability  between  different  robotic  projects  [1 1], 

Fundamentally,  ROS  operates  as  an  object-oriented  messaging  service.  This  allows  commu¬ 
nications  between  different  processes.  These  processes  are  called  nodes  in  ROS,  which  are 
nothing  more  than  software  code  and  drivers.  This  provides  a  great  framework  for  incorporat¬ 
ing  sensors  and  other  devices  into  the  robot.  Incorporating  a  laser  rangefinder  involves  writing 
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a  driver  to  interface  with  the  device  and  then  transmit  the  information  to  other  nodes  that  need 
the  information. 

The  first  mode  of  communications  is  a  traditional  service  style  messaging  system.  One  node 
waits  for  a  signal  from  another  node  before  transmitting  a  response.  This  provider/client  system 
is  effective  but  requires  the  nodes  to  be  more  coupled  than  is  desired  for  this  project. 


Figure  3.10:  Illustration  of  a  basic  ROS  publisher/subscriber  interaction. 


MONTe  uses  a  publisher/subscriber  framework  for  handling  inter-nodal  communications.  The 
framework  is  made  up  of  publisher  nodes,  subscriber  nodes,  and  topics.  Figure  3.10  provides  a 
basic  illustration  of  the  interaction  of  nodes  through  a  topic.  The  publisher  generates  a  message 
(described  by  a  user-defined  .  msg  template)  that  is  then  published  to  a  topic.  The  subscriber 
will  read  from  the  topic,  via  another  message,  by  a  polling  process  called  “spinning”.  The 
publisher  and  subscriber  are  decoupled  because  neither  directly  sees  the  other.  Taking  the  pub¬ 
lisher/subscriber  concept  further,  multiple  nodes  can  subscribe  or  publish  to  the  same  topic.  A 
node  has  the  capability  to  both  publish  and  subscribe.  This  allows  for  complex  interactions 
between  nodes,  and  the  decoupling  makes  debugging  nodes  easier  [20]. 

3.2.2  Functional  Architecture 

MONTe’s  program  has  several  design  goals  for  this  version.  Conceptually,  the  architecture 
needs  to  provide  a  foundation  for  current  and  future  work.  Physically,  it  needs  to  interface  with 
sensors,  control  the  motors,  and  communicate  with  the  operator.  Behaviorally,  it  needs  to  fuse 
the  sensor  data  and  user  inputs  to  navigate  effectively.  Finally,  the  structure  of  the  program 
needs  to  be  de-conflicted  so  that  the  nodes  are  not  interfering. 

Figure  3.11  outlines  the  basic  structure  of  MONTe’s  main  program.  It  illustrates  all  nodes, 
topics,  and  hardware  interfaces.  It  further  illustrates  the  flow  of  information  and  commands 
through  the  network  of  nodes.  A  more  detailed  discussion  of  the  individual  nodes  and  topics 
will  follow. 
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Figure  3.11:  Diagram  of  MONTe  ROS  Architecture 
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ROS  Nodes 

Nodes  are  vital  to  how  ROS  operates.  Fundamentally,  nodes  in  ROS  are  functions  that  perform 
tasks  that  an  autonomous  system  needs  to  accomplish.  Messages  and  topics  allow  each  node  to 
operate  independently,  as  each  only  sees  the  topics  it  is  subscribed  to. 

Figure  3.12  shows  typical  initialization  commands  used  in  a  ROS  node.  Upon  launch,  each 
node  will  invoke  ros  :  :  init  and  ros:  :Node Handle  to  initialize  the  node  and  provide  a 
name  or  “handle”  for  ROScore  to  interact  with.  Next,  all  publishers  and  subscribers  are  set 
up  via  the  advertise/subscribe  functions.  Finally,  the  messages  needed  to  talk  to  the  topics  are 
initialized.  In  this  case,  MONTe  :  :  Plant  -Command  cmd  initializes  a  message  handle  cmd 
of  message  type  Plant-Command .  msg.  This  would  enable  messages  to  be  sent  to  the  ROS 
topic  Plant_Command_T. 

At  this  point  the  node  will  enter  a  loop  to  perform  calculations.  The  first  or  last  item  in  the  loop 
should  be  to  “spin”  ROS  in  order  to  poll  all  topics,  ros  :  :  spinOnce  ( )  polls  any  callback 
functions  that  have  been  declared  in  the  node.  The  node  can  also  publish  at  any  point  of  the 
loop  using  the  .  publish  ( )  command. 
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/*  Sample  code  to  demonstrate  ROS  concepts  */ 

int  main  (  void  ) ; 

{ 

ros  :  :  i  n  i  t  (  argc  ,  argv  ,  ”MONTe_Navigation”  ) ;  //  Set  up  ROS  node 

ros  :  :  NodeHandle  n;  //  Set  up  handle  for  this  node 
//  Set  up  all  publishers  for  node 

ros  ::  Publisher  plt_cmd_pub  =  n  .  advertise  <MONTe:  :  Plant.Command >(”Plant_Command_T”  ,  1); 

//  Set  up  all  subscrivers  for  node 

ros  ::  Subscriber  sub.cf  =  n  .  subscribe  ( ”Command_Flags_T”  ,  1,  Command.FlagsCallback  ) ; 

//  Set  up  message  handles 
MONTe :  :  Plant_Command  cmd; 

MONTe :  :  Command-Flags  flags; 

/*  More  code  here  */ 
while  (  ros  :  :  ok  ( ) ) 

{ 

ros::spinOnce();  //  Poll  topics 

/*  Perform  calculations  */ 

plt_cmd_pub  .  publish  (cmd ) ;  //  Publish  data  to  topic 

} 

} 

void  Command_FlagsCallback  (  const  MONTe ::  Command_FlagsConstPtr&  flags  ) 

{ 

Cmd.Flags  .  autonav  =  flags— >  auto.nav ; 

Cmd.Flags  .  mode  =  flags— >  nav.mode  ; 

Cmd.Flags  .  route  =  flags— >  incoming-route; 

}  //  end  callback 


Figure  3.12:  Sample  code  illustrating  ROS  concepts 

MONTe’s  nodes  can  be  conceptually  divided  into  two  types:  demand  and  continuous.  The 
demand  nodes  consist  of  the  Waypoint  Control  and  Keyboard  Control.  Thesedonot 
run  in  a  continuous  loop,  instead  wait  for  input  prior  to  executing.  The  other  nodes  are  designed 
to  be  running  in  a  continuous  loop.  These  nodes  will  run  processes,  and  poll  and  publish  to  topic 
at  a  rate  of  4  Hz.  The  nodes  do  this  by  taking  advantage  of  the  ROS  loop.ratesleep  ( ) 
function  that  enforces  the  desired  frequency.  The  system  cycle  rate  of  4Hz  was  selected  based 
on  updating  the  navigation  algorithm  at  a  sufficient  rate,  but  is  open  to  further  optimization. 

Communications  Node 

This  node  covers  the  communication  with  the  base  station  and  operator.  The  node  communi¬ 
cates  via  UDP  protocol  over  a  series  of  ports  to  keep  data  streams  separate.  The  communication 
types  are  divided  into  a  series  of  channels.  Each  channel,  upon  receipt  or  transmission,  will 
publish  data  and  control  flags  to  various  topics.  An  example  would  be  manual  control.  Upon 
receipt  of  a  manual  command,  the  Communications  node  would  process  the  input  and  pub¬ 
lish  the  pertinent  data  to  ROS  topics  like  Plant.Control  topic  and  CommancLFlags  . 
The  other  communication  channels  include  Waypoints  (receive),  Navigation  (send),  and 


21 


Message  (send). 


Waypoints  Node 

The  Waypoint  node  processes  operator  generated  navigation  data.  The  waypoints  are  stored 
in  the  node  using  dynamically  allocated  memory,  and  are  arbitrarily  limited  to  ten  waypoints. 
The  stack  has  functionality  to  switch  between  waypoints,  create  more  and  to  delete  the  stack. 


Waypoints  node  subscribes  to  the  New_Waypoint  and  Command-Flags  topics  to  manip¬ 
ulate  and  input  new  waypoints  into  the  stack.  The  node  publishes  the  current  destination  to  the 

Current.Waypoint s  topic. 


Figure  3.13:  Flowchart  for  MONTe’s  Navigation  Node 
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Navigation  Node 

The  Navigation  node  is  the  primary  behavior  generator  for  MONTe.  In  Figure  3.11,  Navigation 
resides  at  the  center  to  represent  its  importance  in  the  overall  architecture.  All  major  data  paths 
begin  or  end  with  the  navigation  node. 

The  current  program  will  allow  MONTe  to  travel  to  a  desired  waypoint  without  object  avoid¬ 
ance.  Figure  3.13  details  the  operation  of  the  navigation  routine.  Navigation  polls  Command-Flags 
and  Cur rent -Waypoints  to  verify  MONTe  is  in  auto-nav  mode  while  updating  the  next 
destination.  Positional  data  (compass  and  GPS)  is  received  from  the  Nav_Data  topic.  Cur¬ 
rent  position  and  the  current  waypoint  are  compared  and  appropriate  plant  control  data  is  pub¬ 
lished  to  Pi  ant -Control  topic.  When  the  current  destination  is  reached  a  flag  is  updated  on 
Command-Flags  so  that  the  Waypoint  node  can  send  the  next  waypoint. 

Monkey  Node 

The  Monkey  node  is  the  driver  associated  with  the  Monkey  Attitude  Heading  Reference  System 
(AHRS)  unit.  In  the  current  implementation,  the  node  is  a  publisher  only.  Its  primary  function 
is  to  receive  GPS,  compass,  and  velocity  data  for  publishing  to  the  NavJCata  topic.  In  the 
future,  its  secondary  function  is  to  allow  for  manual  control  of  the  tail  or  adjust  the  autonomous 
functions  of  the  tail. 

Plant  Control  Node 

The  Plant  Control  node  is  simple  in  operation.  It  is  the  driver  for  interfacing  with  the 
Sabertooth2xl2  motor  drivers.  It  handles  this  by  polling  the  Plant-Control  topic  to  receive 
commands.  They  are  then  parsed  and  transmitted  via  RS-232  serial  port  to  the  motor  drivers. 

Keyboard  Control  Node 

Keyboard  Control  allows  the  user  to  control  MONTe  manually  via  a  virtual  network  client 
(VNC)  server.  Control  is  rudimentary  with  forward,  reverse,  stop,  left,  and  right  turns  possi¬ 
ble.  It  also  allows  the  speeds  and  turning  rates  to  be  adjusted  during  operation.  Upon  receipt 
of  a  command,  the  node  will  publish  the  motor  commands  to  the  Plant-Control  topic, 
while  updating  the  Command-Flags  topic  to  manual  control.  This  will  take  MONTe  out  of 
autonomous  navigation. 

Waypoint  Control  Node 

Waypoint  Control  allows  the  user  to  input  and  delete  waypoints,  and  then  execute  the 
route  remotely  via  VNC.  The  maximum  number  of  waypoints  is  set  to  ten.  Each  waypoint  is 
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#  Plant.Command  . 

# 

msg 

fr 

#  Basic  message 

for 

manually  controllin 

g  MONTe  in  simplified  serial  mode 

#  Speed  command 
uint8  left 

for 

left 

motor  . 

Range 

is  1  (  Full  Reverse)— >  64  (Stop)  <— 

127  (Full  Forward) 

#  Speed  command 
uint8  right 

for 

left 

motor  . 

Range 

is  128(Full  Reverse)— >  192  (Stop) 

<—  255  (Full  Forward) 

Figure  3.14:  Example  of  a  ROS  message 


set  up  with  latitude,  longitude  (in  decimal  degrees),  waypoint  number,  and  an  action.  Actions 
are  set  up  for  future  use  for  more  sophisticated  control.  Once  a  route  is  generated,  the  user  will 
send  the  route  which  feeds  into  the  New.Waypoint  topic.  CommancLFlags  topic  will  also 
be  updated  to  autonomous  navigation  which  will  take  MONTe  out  of  manual  control. 

ROS  Topics 

Topics  allow  ROS  to  transmit  data  between  nodes.  The  nodes  only  see  the  topics  they  publish 
and  subscribe  to,  which  decouples  them.  Each  topic  is  communicated  to  and  from  via  a  message. 
Valid  data  types  include  integers,  floating  point,  characters,  and  strings.  A  sample  message  is 
shown  in  Figure  3.14.  This  simple  message  holds  to  unsigned,  8-bit  integers,  and  can  be  used 
to  send  to  or  receive  from  a  topic. 

The  main  type  of  topics  that  MONTe  utilizes  transfer  sensor  data  between  the  nodes.  For 
example,  NavJData  provides  the  current  position  and  heading  of  MONTe  to  any  node  that 
requires  that  data.  Other  messages  transfer  command  data  to  topics  such  as  New.Waypoint 
and  Current_Waypoint.  The  messages  in  this  case  send  waypoints  with  any  additional 
information  required  for  processing  the  data. 

CommancLFlags  topic  is  a  special  topic  that  stores  all  behavioral  flags  that  control  MONTe’s 
operation.  The  current  flags  it  stores  are  auto.nav,  navjnode,  and  incoming_route. 
These  flags  allow  MONTe  to  switch  between  manual  and  autonomous  control  modes,  indicate 
when  a  waypoint  is  reached,  and  to  warn  when  a  new  waypoint  route  is  in  the  queue. 

3.3  Control  System  Hardware 

The  control  system  hardware  is  housed  in  the  main  body  of  MONTe,  shown  in  Figure  3.15. 
Individual  components  are  mounted  onto  a  power  module  that  acts  to  support  the  electronics 
and  route  wiring  for  the  devices.  This  assembly  also  organizes  the  switches  that  activate  each 
power  bus  and  device. 


24 


Figure  3.15:  Picture  of  the  internal  design  and  component  placement  of  MONTe 


3.3.1  Main  Processor 


Figure  3.16:  Picture  of  the  Stealth  LPC-100 

The  main  computational  device  for  MONTe  is  a  Stealth  LPC-100  mini-personal  computer,  Fig¬ 
ure  3.16.  It  was  selected  due  to  its  small  size.  It  weighs  1.2  lbs,  and  its  dimensions  are  4.0”(W) 
x  6.1”(D)  x  1.45”(H).  This  small  form  factor  is  ideal  for  use  in  a  self-contained  robot. 

The  computer  runs  on  a  1.9GHz  Intel-Celeron  processor  with  4GB  of  RAM.  This  provides  a 
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robust  capability  for  running  the  operational  program,  as  well  as  serving  as  the  communications 
hub.  The  operating  system  is  Linux/Ubuntu  10.04  that  allows  a  stable  version  of  ROS  to  be  run. 

3.3.2  Monkey  Board 


Figure  3.17:  Picture  of  the  2010  Monkey  Board  produced  by  Ryanmechatronics  LLC 

The  Monkey  2010  platform,  Figure  3.17,  is  a  versatile  circuit  assembly  that  can  be  used  to 
control  autonomous  vehicles.  There  are  multiple  capabilities  inherent  to  the  board,  but  some 
of  the  main  functions  include:  a  Cortex  M3  (ARM  7)  processor,  U-Blox  NEO-5  GPS  module, 
barometric  pressure  sensor,  EO  ports,  pulse  width  modulation  (PWM)  servo  outputs,  and  status 
LEDs.  This  board  is  designed  to  accompany  a  CHIMU  (product  name)  AHRS.  A  robust  soft¬ 
ware  suite  allows  for  easy  user  interface  and  control  algorithm  development.  Combining  these 
modules  allows  MONTe  to  acquire  its  GPS  position  and  know  its  spatial  orientation:  roll,  pitch, 
and  yaw.  The  Monkey  is  limited  in  its  video  processing  capabilities  and  therefore  only  used  for 
advanced  sensing  and  tail  control. 

3.3.3  Motor  Driver 

The  Sabertooth2xl2,  shown  in  Figure  3.18,  is  a  6-24V  motor  driver  designed  for  analog,  radio 
control  (RC)  and  serial  control  applications.  It  can  operate  at  up  to  12A  continuously  and  control 
two  sets  of  motors  per  channel.  Serial  control  in  simplified  mode  was  selected  for  controlling 
MONTe.  Single  byte  commands  are  sent  via  RS-232  protocol  to  individually  control  each  set 
of  Whegs™.  The  Sabertooth2xl2  has  the  capability  to  control  up  to  eight  different  channels 
simultaneously  using  packetized  serial  mode.  Finally,  the  motor  driver  can  operate  in  a  ramping 
mode  that  provides  smooth  acceleration  upon  receipt  of  commands  [21]. 
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Figure  3.18:  Picture  of  the  Sabertooth  2x12  motor  controller 


Figure  3.19:  Picture  of  the  Belkin  Wireless  Adapter 


3.3.4  Network  Card 

Wireless  communications  is  provided  by  a  Belkin  N  Wireless  Adapter  (F5D8053),  Figure  3.19. 
The  USB  device  has  data  rates  of  up  to  300Mbps  under  USB2.0  interface.  New  drivers  were 
necessary  to  get  the  device  to  work  correctly  under  Ubuntu  10.04. 


3.3.5  CMUcam3 

The  CMUcam3,  Figure  3.20,  is  a  (352x288)  RGB  color  camera  designed  for  open  source  devel¬ 
opment  for  a  variety  of  applications.  It  is  capable  of  performance  up  to  26  fps,  and  can  process 
images  onboard  prior  to  downloading  to  another  computer.  The  CMUcam3  will  be  used  for 
future  implementation  of  robotic  stereovision  for  obstacle  avoidance. 
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Figure  3.20:  Picture  of  the  CMU  camera 


MONTe 

Power  Bus  Architecture 


—Control  System  •  '  Drive  System- 

Figure  3.21 :  MONTe  Power  Bus  Architecture 


3.4  Power  Bus 

MONTe’s  power  bus  was  designed  to  power  both  the  control  hardware  (processor,  sensors),  and 
the  drive  hardware.  Furthermore,  the  power  bus  provides  the  appropriate  voltage  regulation  and 
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protection  necessary  for  operation  of  MONTe.  The  overall  architecture  is  detailed  in  Figure 
3.21.  The  power  for  the  bus  is  provided  by  stacks  of  Lithium  Polymer  (LiPo)  batteries.  Each 
stack  provides  14.4V  to  the  bus.  A  single  stack  powers  the  Control  System,  while  two  stacks 
wired  in  parallel  power  the  Drive  System. 

The  Control  System  consists  of  the  LPC-100  computer,  and  the  Monkey  navigational  unit.  The 
LPC-100  is  fed  off  a  RCB-DCDC006  12V  Regulator.  The  Monkey  is  fed  by  a  0J2334  Pololu 
5V  Regulator.  Future  sensors  and  control  hardware  will  be  incorporated  on  this  bus. 

The  Drive  System  consists  of  the  Sabertooth2xl2  (unregulated),  and  the  HS-7955  TG  Tail  Servo 
Motors.  The  servo  motors  are  powered  by  three  parallel  Pololu  5V  Regulators.  This  provides 
the  necessary  current  to  operate  the  tail  in  self-righting  mode.  The  two  LiPo  battery  stacks  are 
in  parallel  to  provide  extended  operating  time. 

3.5  Communication  Paths 

Transferring  data  is  a  vital  component  of  any  autonomous  or  unmanned  system.  MONTe  uses  a 
variety  of  paths  for  external  and  internal  communications  detailed  in  Figure  3.22.  Both  current 
implementation  (solid  lines),  and  future  communications  paths  (dashed  lines)  are  illustrated. 
The  LPC-100  serves  as  the  central  hub  for  both  the  internal  communication  paths  (hardware), 
and  for  external  communications  with  the  base  station. 

3.5.1  Internal  Paths 

The  internal  communication  paths  for  MONTe  are  to  transfer  information  between  the  different 
sensors  and  processors.  The  two  main  formats  utilized  are  RS-232  serial  communications  and 
User  Datagram  Protocol  (UDP)  communications. 

UDP  is  an  Internet  Protocol  that  operates  in  datagram  mode.  This  provides  a  quick  way  of 
sending  packets  of  information  both  over  hard  connection  (cross-over  cable)  or  via  wireless. 

Motor  Control 

The  LPC-100  communicates  with  the  Sabertooth2xl2  motor  drivers  via  RS-232  over  COM1 
port.  The  motor  drivers  are  operated  in  simplified  serial  mode  and  receive  command  bytes  for 
each  individual  motor.  The  RS-232  signal  must  be  stepped  down  to  0-5V  TTL  signal  via  an 
optical  isolation  circuit.  The  implementation  is  accomplished  via  a  custom  C++  library  based 
on  code  from  Reference  [22] . 
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Monkey 

The  transfer  of  navigation  data  and  flags  is  handled  by  a  serial-USB  interface.  This  allows  better 
allocation  of  serial  communication  ports  on  the  LPC-100.  GPS  data  is  sent  in  NMEA  format. 
Compass  data  and  control  flags  are  also  sent  over  the  path. 


CMUcam3 

The  stereovision  system  will  be  integrated  in  the  future  via  RS-232  on  COM2.  One  camera 
will  be  the  master  and  be  connected  via  its  UART  to  COM2.  The  slaved  CMUcam3  will  be 
connected  via  the  second  general  purpose  input/output  port  (GPIO)  on  the  master  CMUcam3. 


Microcontroller 

MONTe  has  the  functionality  to  connect  with  a  microcontroller  via  UDP  using  a  cross-over 
cable.  This  provides  a  quick  way  of  incorporating  a  microcontroller  for  integrating  additional 
sensors.  The  communication  protocol  is  designed  to  work  with  a  Rabbit  BL4S200  micropro¬ 
cessor. 
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VNCserver 

MONTe  runs  a  VNCserver  in  the  background  so  the  operator  can  make  adjustments  while  ac¬ 
tive.  Changes  to  the  program  and  node  architecture  can  be  performed  depending  on  needs  of 
testing  or  the  mission. 

3.5.2  External  Paths 

External  communications  is  how  MONTe  will  interact  with  the  operator  during  testing  and 
missions.  MONTe  utilizes  UDP  IP  communications  with  the  base  station  through  the  wireless 
USB  adapter.  UDP  is  simple  to  implement  and  requires  less  processor  overhead  from  the  LPC- 
100.  This  will  allow  MONTe  to  receive  commands,  and  transmit  sensor  data  to  and  from  the 
base  station. 
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CHAPTER  4: 
Results 


4.1  Component  Interfaces 

4.1.1  Motor  Driver 

The  Sabertooth2xl2  dual  12A  motor  driver  was  tested  in  both  simplified  and  packetized  serial 
modes.  The  Sabertooth2xl2  ramping  function  was  also  tested  to  show  a  smooth  acceleration 
of  the  motors.  Simplified  serial  mode  was  implemented  in  the  design  due  to  its  simplicity  and 
effectiveness.  However,  simplified  serial  was  found  to  be  incompatible  with  the  ramping  feature 
of  the  Sabertooth2xl2.  Simplified  serial  requires  individual  commands  for  each  set  of  motors. 
It  was  found  that  sending  the  command  for  the  second  motor  would  override  the  first  command 
if  it  had  not  reached  the  ordered  speed. 

MONTe  is  currently  working  in  simplified  serial  with  no  ramping  function  enabled.  MONTe 
can  effectively  move  using  this  mode.  The  lack  of  ramping  is  of  concern  due  to  wear  on  the 
drive  train.  However,  this  is  acceptable  for  the  current  iteration. 

Packetized  serial  would  solve  this  issue,  but  operation  in  that  mode  was  not  consistent.  The  first 
couple  of  commands  to  the  plant  would  work  as  expected.  However,  errors  would  creep  in  to 
the  motor  driver  as  the  values  changed  yielding  nondeterministic  behavior.  Packetized  serial 
will  be  necessary  for  implementation  of  the  water  jets,  so  further  research  and  development  is 
necessary. 


4.1.2  Navigation  Data 

The  interface  between  the  CHIMU  AHRS  and  the  main  program  on  the  LPC-100  is  done  via 
RS-232  protocol  at  115,200  Baud.  Navigation  data  was  successfully  received  using  buffered 
serial  at  a  rate  of  4  Hz.  The  main  data  transferred  includes:  the  number  of  satellites  tracked,  fix 
quality,  three-dimensional  velocity,  altitude,  and  heading.  Latitude  and  longitude  are  in  decimal 
degrees  to  four  places.  4Hz  was  selected  to  synchronize  with  the  rest  of  the  ROS  program 
architecture  to  provide  a  constant  update  of  heading  information  for  the  navigational  control 
and  could  be  further  optimized  in  the  future. 
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Comparison  of  Compartment  Temperature  for  Bench  Test#1 
and  #2 


Time  {min) 


Figure  4.1 :  Comparison  of  compartment  temperature  profile  characterizations 


Bench  Test  #2:  Temperature  Rise  vs  Time 
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Figure  4.2:  Compartment  temperature  change  over  time. 


4.2  Compartment  Temperature  Profile 

Because  the  LPC-100  computer  has  a  recommended  maximum  operating  temperature  of  40°C, 
two  bench  tests  were  performed  to  determine  a  reasonable  high-end  limit  for  continuous  op¬ 
eration  while  the  compartment  was  sealed.  The  LPC-100  was  started  inside  the  chassis  and 
employed  a  continuous  counting  program  to  stress  the  processor.  The  compartment  tempera¬ 
ture  was  monitored  during  operation  after  the  compartment  was  sealed.  The  first  test  was  done 
with  the  LPC-100  running  off  of  external  power,  while  the  second  was  run  off  a  14.4V  Lithium 
Polymer  battery  stack  in  the  design  configuration. 
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Figures  4.1  and  4.2  shows  the  compartment  temperature  profile  generated  by  both  bench  tests. 
The  initial  internal  compartment  temperature  for  both  tests  was  22°C.  The  first  test  took  32 
minutes  to  reach  the  critical  temperature,  while  the  second  bench  test  took  only  28  minutes 
on  battery  power.  The  inclusion  of  internal  power  dropped  the  time  by  5  minutes  and  proved 
that  the  temperature  profile  is  a  concern.  Future  work  will  include  heats  sinks  to  manage  the 
compartment  temperature. 


4.3  Mobility 

4.3.1  Self-Righting 

Modeling  Environment 

One  of  the  major  advancements  of  MONTe  is  the  incorporation  of  an  autonomous  tail.  To 
anticipate  the  behavior  of  the  robot  and  improve  the  design  process,  MONTe  was  modeled 
using  Working  Model  2D  (WM2D).  This  simulation  environment  models  Newtonian  equations 
of  motion  for  interacting  bodies  and  displays  the  output  in  an  intuitive  user  interface  [23].  It 
allows  for  interactive  simulations  that  can  receive  input  from  user  controls,  scripts,  and  other 
applications,  such  as  Excel  and  MATLAB.  One  drawback  is  that  the  software  only  models  in 
two  dimensions  and  therefore  does  not  allow  for  three  dimensional  terrain  and  assumes  stability 
in  the  roll  direction.  MONTe  was  modeled  using  approximate  geometries  and  weights.  This 
modeling  environment  provides  a  proving  ground  for  various  designs  and  control  algorithms 
without  requiring  a  test  platform. 


Figure  4.3:  WM2D  Model  of  MONTe  rotating  its  tail  from  the  neutral  position  to  the  stow  position  while  upside  down 

Self-righting  creates  the  most  limiting  condition  for  the  required  torque  of  the  servo  drive  mech¬ 
anism.  MONTe  was  modeled  early  in  the  design  phase  to  estimate  the  necessary  gear  ratio  for 
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Figure  4.4:  WM2D  Model  of  MONTe  rotating  its  tail  from  the  stow  position  to  the  neutral  position  while  upside  down 

the  servo.  There  are  two  different  scenarios  in  which  the  servo  drive  mechanism  would  be 
subjected  to  a  large  load.  Figure  4.3  shows  a  model  of  MONTe  righting  itself  from  an  initial 
neutral  tail  position.  As  the  tail  is  retracting  to  the  stow  position,  the  servo  drive  mechanism  is 
exerting  a  maximum  torque  of  approximately  60  lb-in.  This  corresponds  to  960  oz-in.  Figure 
4.4  shows  a  similar  model  except  the  tail  is  initially  stowed.  This  is  the  most  limiting  situation 
and  requires  the  highest  torque  from  the  servo  drive  mechanism.  The  maximum  magnitude  of 
torque  required  is  approximately  85  lb-in,  which  corresponds  to  1400  oz-in.  This  value  closely 
agreed  with  the  torque  expected  from  our  design  criteria  and  a  simple  lever  arm  from  Section 
3.1.6. 

The  two  gears  selected  for  use  in  MONTe  have  torque  ratings  corresponding  to  1250  oz-in  or 
2150  oz-in,  based  on  supply  voltage  of  4.8  V  [18].  Selection  of  a  specific  servo  drive  mech¬ 
anism,  with  either  a  5:1  or  8.6:1  gear  ratio,  places  a  constraint  on  the  servo  drive  speed.  The 
rotation  speed  will  either  be  approximately  45  or  75  °/sec  based  on  the  same  supply  voltage. 
This  rotation  speed  will  dictate  the  time  it  takes  for  MONTe  to  right  itself,  but  will  also  impact 
the  time  response  of  the  tail  when  climbing  obstacles.  Since  the  model  shows  that  MONTe 
requires  at  least  1400  oz-in,  the  8.6:1  gear  ratio  will  be  necessary  to  right  MONTe. 

Field  Trials 

MONTe  was  subjected  to  tests  to  determine  if  self-righting  would  be  possible.  At  the  time  of 
testing  MONTe  weighed  19  pounds,  just  below  the  anticipated  weight  of  20  pounds.  Initial 
tests  showed  that  MONTe  intermittently  succeeded  to  flip  with  a  5:1  gear  system  installed.  The 
initial  reason  for  failure  was  due  to  reaching  the  upper  limit  on  current  supplied  to  the  servo  drive 


36 


mechanism.  Additional  regulators  were  placed  in  parallel  to  provide  sufficient  current.  Success 
with  the  5:1  gear  ratio  improved,  however  it  was  still  intermittent  and  required  upgrading  to  the 
8.6:1  gears.  Tests  were  then  conducted  with  repeated  success  using  the  8.6:1  gearratio.  Figure 
4.5  shows  MONTe  successfully  righting  itself  with  those  gears  installed.  This  scenario  is  from 
the  stowed  position  and  places  the  servo  drive  mechanism  under  the  largest  load.  Success  from 
this  position  indicates  that  MONTe  will  be  able  to  right  itself  from  any  variation  of  the  scenario. 
This  also  ensures  that  there  is  substantial  torque  available  to  lift  the  rear  of  MONTe  for  climbing 
assist. 


Figure  4.5:  Clips  of  video  showing  MONTe  righting  itself  from  the  limiting  scenario 


4.3.2  Climbing  Assist 

Modeling  Environment 

MONTe  was  again  modeled  in  order  to  develop  an  algorithm  to  control  the  tail  for  climbing 
assistance.  As  discussed  in  Section  2.1.2,  high  centering  is  the  common  failure  for  similar  surf- 
zone  designs.  In  order  to  analyze  this  high  centering  condition  MONTe  was  modeled  climbing  a 
six  inch  step,  Figure  4.6.  This  screen  shot  from  WM2D  includes  control  parameters  and  output 
data.  Figure  4.7  highlights  the  pitch  data  of  the  main  body  during  the  stalled  climb.  The  slope  of 
the  pitch  graph  corresponds  to  the  rate  of  change  of  that  pitch,  which  nearly  drops  to  zero  when 
MONTe  becomes  high  centered.  This  phenomenon  is  used  to  invoke  the  tail  for  assistance. 

The  control  algorithm,  Appendix  A.  1  and  A. 2,  was  then  developed  in  MATLAB  based  on  the 
adverse  pitch  rate  during  high  centering.  This  control  method  actuates  the  tail  in  the  event 
MONTe  develops  a  high  average  pitch  rate  (>  4°/sec)  followed  by  an  low  instantaneous  pitch 
rate  (<  l°/sec).  MONTe  was  then  modeled  on  the  same  six  inch  step,  but  now  linked  to  the  new 
control  algorithm  in  MATLAB.  Figure  4.8  shows  MONTe  successfully  climbing  the  previous 
obstacle.  This  algorithm  was  then  programmed  onto  the  Monkey  board  for  real  obstacle  testing. 
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Figure  4.6:  Model  of  MONTe  encountering  a  high  center  condition 
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Figure  4.7:  Pitch  data  from  a  high  center  scenario 


Since  the  tail  control  algorithm  was  written  in  MATLAB  based  on  WM2D,  some  modifications 
were  necessary  to  host  the  algorithm  on  the  Monkey  board.  Migrating  the  control  program  from 
MATLAB  to  the  Monkey  in  C  language  required  two  major  changes.  First,  the  program  would 
output  desired  tail  angle  vice  tail  speed.  WM2D  does  not  contain  servo  motors  as  devices  and 
only  allows  control  of  rotational  motors  through  torque,  speed,  or  position  values.  When  using 
the  position  method  it  would  instantaneously  move  the  tail  to  the  new  prescribed  angle.  In 
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Figure  4.8:  MONTe,  in  WM2D,  interfaced  with  MATLAB  tail  control  algorithm  to  overcome  an  obstacle 

order  to  overcome  this  inaccuracy,  tail  speed  was  controlled  vice  angular  position.  For  the  real 
servo,  the  desired  angle  is  prescribed  through  PWM  and  the  inherent  servo  electronics  control 
the  actual  speed  to  arrive  at  that  angle.  Therefore  the  output  was  revised  to  control  tail  position. 
Secondly,  the  real  AHRS  presents  additional  noise  beyond  that  seen  in  the  pitch  rate  of  Figure 
4.6.  Since  this  noisy  signal  of  raw  pitch  rate  had  the  potential  for  creating  erroneous  tail  control, 
the  pitch  rate  was  instead  calculated  from  the  pitch  angle.  The  sampling  rate  for  the  pitch  angle 
was  set  and  used  to  calculate  an  average  pitch  rate.  This  time  average  was  used  in  lieu  of  raw 
pitch  rate  and  successfully  suppressed  the  erratic  signal.  The  Monkey  control  algorithm  can  be 
found  in  Appendix  B . 

Field  Trials 

Field  trials  were  conducted  with  the  modified  control  algorithm.  MONTe  was  successful  at 
actuating  the  tail  when  the  front  was  lifted  to  simulate  a  stall  condition.  Additional  testing  was 
conducted  while  driving  MONTe  over  obstacles  manually.  For  these  tests  the  tail  was  con- 
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Performance  Test  #1: 
Velocity  vs  Command 
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Figure  4.9:  Performance  curves  over  concrete 

trolled  autonomous  by  the  Monkey  board.  MONTe  repeatedly  failed  to  climb  step  obstacles 
after  multiple  attempts.  During  this  testing  phase,  several  design  issues  were  noted.  The  sus¬ 
pension  system  was  not  stiff  enough  and  caused  MONTe  to  travel  low  as  it  reached  an  obstacle. 
The  Whegs™were  also  not  equipped  with  any  form  of  traction  and  prevented  MONTe  from 
advancing  onto  the  obstacle  once  the  tail  was  lowered.  Additional  testing  will  be  required  to 
prove  MONTe’s  overall  design  and  in  order  to  tune  the  tail  control  algorithm,  see  Section  5.1. 

4.3.3  Speed  of  Advance 

MONTe  was  tested  over  concrete  to  develop  the  motor  control  algorithms.  Data  was  taken  over 
a  full  range  of  control  signals  for  forward  velocity.  Results  are  detailed  in  Figure  4.9. 

Maximum  velocity  was  determined  to  be  3.4  m/s  for  simplified  serial  command  of  64.  A  nom¬ 
inal  traveling  velocity  of  1.6  m/s  was  selected  (serial  command:  30).  The  maximum  turning 
differential  (without  slipping)  was  10. 

4.4  ROS  Testing 

ROS  testing  explored  the  capabilities  of  the  operating  system  in  manual  control  and  autonomous 
navigation.  MONTe’s  program  was  successfully  able  to  operate  in  full  capacity.  Figure  4.10 
shows  the  current  architecture  using  the  rxgraph  utility  provided  by  ROS.  Each  active  node 
is  displayed  with  the  connecting  topics  between  nodes  illustrated  as  well. 

Each  connection  represents  a  subscribe,  or  publish  path  through  the  labeled  topic.  This  pro- 
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Figure  4.10:  Output  of  ROS  rxgraph  function  showing  MONTe’s  node  architecture 

vides  an  easy  display  of  the  dependencies  of  each  node.  The  Waypoint  Control,  and  Key- 
boaixLControl  were  both  successful  in  transmitting  user  commands  to  the  rest  of  the  system. 

4.4.1  Remote  Control 

Remote  operation  of  MONTe  used  a  VNCserver  for  remote  access  during  operation.  The  tests 
were  successful  as  different  program  nodes  could  be  started  or  stopped  over  a  wireless  network. 

For  example,  MONTe  could  be  run  in  manual  control  only  by  starting  ROScore,  Keyboard 
Control,  and  Plant  Control  nodes.  This  also  allowed  changing  and  recompiling  ROS  nodes 
during  testing.  This  proved  useful  during  calibration  of  plant  control  constants. 

MONTe  could  be  remotely  piloted  as  well.  By  running  the  nodes  Keyboard.Control  and  Plant.Control, 
MONTe  could  be  given  commands  to  go  forward,  reverse,  turn  left,  and  turn  right.  The  forward 
and  reverse  velocities,  and  turning  rates  could  be  adjusted. 

Unacceptable  latency  was  encountered  during  remote  operation.  Lag  times  of  upwards  of  15- 
30  seconds  were  observed  in  processing  commands  and  receiving  program  responses  while  on 
VNC.  MONTe  would  be  unresponsive  to  commands  during  these  periods  of  lag.  This  was 
not  an  issue  during  autonomous  navigation,  but  did  pose  a  significant  issue  when  in  manual 
control.  Latency  was  not  unexpected,  though,  and  will  drive  future  efforts  to  perform  most 
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control  via  Java-based  GUI.  Relegating  the  VNC  to  providing  means  of  deep  modifications  of 
the  operational  program  will  relieve  the  processor  and  optimize  performance. 

A  secure  shell  protocol  (SSH)  is  another  option  for  remote  operation.  This  only  brings  up  a 
unix  shell  for  interacting  with  the  system,  so  has  less  overhead  associated  with  it.  Multiple 
SSH  connections  could  be  made  to  bridge  the  gap  until  a  more  sophisticated  interface  can  be 
implemented. 
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CHAPTER  5: 
Future  Work 


5.1  Improvements  to  an  Autonomous  Tail 

5.1.1  Tail  Design 

While  modeling  MONTe  and  conducting  field  tests,  several  design  aspects  were  considered 
problematic.  In  one  scenario  the  cross  member  for  the  tail  protrudes  too  far  below  the  chassis 
height.  This  limited  ground  clearance  caused  a  unanticipated  stall  condition  and  required  cy¬ 
cling  the  tail  in  order  to  free  the  tail.  The  cross  member  was  originally  included  in  the  design  in 
order  to  add  rigidity  to  the  tail  and  prevent  it  from  twisting.  Further  testing  should  be  conducted 
on  the  tail  to  either  relocate  or  remove  the  cross  member  from  the  design.  Additionally  it  may 
be  possible  to  remove  any  support  between  the  two  sides  of  the  tail  and  instead  drive  them  as 
individual  units.  This  would  no  longer  require  tuning  the  servo  drive  mechanisms  to  ensure 
that  they  are  operated  in  tandem.  This  would  also  allow  the  tail  to  be  controlled  in  a  manner  to 
induce  a  roll  change  as  well  as  a  pitch  change. 

Some  testing  scenarios  also  demonstrated  that  the  friction  at  the  end  of  the  tail  sometimes  lim¬ 
ited  the  forward  motion  of  the  robot.  If  the  tail  was  invoked  to  assist  with  climbing  on  soft 
terrain,  there  was  a  chance  that  the  tail  would  become  almost  embedded  in  the  ground  as  it 
acted  to  lift  the  rear  of  the  robot.  The  limited  surface  area  of  the  tail  tip  and  the  high  friction 
could  cause  a  stall.  It  may  be  possible  to  reduce  the  susceptibility  to  this  condition  by  adding  a 
component  to  the  tail  that  would  instead  roll  along  the  ground.  This  concept  would  reduce  the 
friction  at  the  tail  end  and  allow  the  traction  from  the  Wheg™  to  pull  the  robot  in  the  desired  di¬ 
rection  of  travel.  Future  testing  should  investigate  the  multitude  of  these  design  improvements. 

5.1.2  Tail  Control  Algorithm 

Currently  the  tail  control  program  only  uses  the  pitch  data  from  the  Monkey’s  CHIMU  module 
to  detect  a  high  center  condition  and  to  actuate  the  tail.  Additional  sensors  can  be  used  to 
more  accurately  anticipate  and  detect  a  high  centering  condition.  One  would  be  to  measure 
the  force  exerted  on  a  Wheg™.  When  that  force  goes  to  zero,  the  Wheg™  can  be  considered 
unloaded.  This  can  be  easily  sensed  by  limit  switches  attached  to  the  suspension  inside  the  drive 
assemblies.  Also,  the  CHIMU’s  accelerometer  data  can  be  used  to  determine  if  forward  motion 
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has  ceased,  indicating  a  stall  condition.  This  would  require  interfacing  with  plant  control  to 
ensure  that  the  stall  is  not  intended. 


5.2  Improvements  to  Program  Architecture 

The  current  architecture  will  serve  as  the  framework  for  future  development  of  MONTe.  Many 
ROS  concepts  have  been  proven  and  implemented  such  as  the  publisher/subscriber  method  of 
intermodal  communications.  The  successful  integration  of  sensors  and  hardware  such  as  the 
Monkey,  and  the  Sabertooth2xl2  shows  the  usefulness  of  ROS  for  developing  robotic  projects. 

Numerous  features  can  be  improved  or  added  to  in  the  existing  architecture.  The  current  code 
was  developed  using  concepts  from  both  C  and  C++.  The  overall  architecture  is  object-oriented. 
Nodes  are  essentially  “objects”  that  perform  task,  and  communicate  with  others.  Some  of  the 
legacy  code  utilized  in  development  from  previous  projects  was  written  in  C,  and  is  therefore 
not  object-oriented.  A  traditional,  linear  based  programming  can  easily  be  at  the  mercy  of  bugs 
that  appear  in  the  system,  and  its  nature  decreases  its  “portability”.  Portability  allows  code  to 
be  used  in  a  variety  of  uses,  and  is  one  of  the  main  goals  of  ROS.  Therefore,  the  code  needs  to 
be  modified  in  accordance  with  the  principles  of  object  oriented  programming. 

The  goal  would  be  then  to  modify,  and  conduct  partial  rewrites  of  the  individual  nodes  them¬ 
selves.  This  would  bring  the  code  in  line  with  the  principle  of  object  oriented  programming. 

5.3  Navigation,  Mapping,  and  Object  Avoidance 

Future  work  in  navigation,  mapping  and  object  avoidance  will  include: 

1 .  Implement  additional  sensors  to  improve  navigation  and  autonomy. 

2.  Test  navigational  algorithms  to  optimize  performance. 

3.  Utilize  stereovision  for  object  localization  and  avoidance. 

Proposed  sensors  would  include  laser  range-finders,  acoustic  sensors,  and  stereovision. 

Previous  work  by  Baravik  [24]  developed  algorithms  to  conduct  edge-detection  and  subsequent 
ranging.  His  process  started  by  identifying  the  contours  in  the  scene.  These  contours  are  then 
correlated  to  pick  out  the  objects.  The  range  is  then  determined  by  getting  an  angle  to  pixel  of 
highest  correlation  by  finding  the  pixel  separation.  The  code  was  not  implemented  for  real-time 
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detection  due  to  limitation  of  the  camera  and  microprocessor.  MONTe,  however,  has  greater 
computational  capacity  in  the  LPC-100.  Research  could  be  done  to  develop  a  ROS  node  that 
could  image  the  environment  and  process  the  results  for  object  detection[24]. 

Path  planning  goes  hand  in  hand  with  object  detection.  A  simple  algorithm  is  the  “bug”  al¬ 
gorithm.  MONTe  determines  the  heading  to  the  current  destination  and  drives  the  path  until 
discovery  of  an  obstruction.  MONTe  could  then  follow  the  perimeter  of  the  obstacle  until  clear. 
The  robot  would  then  resume  its  heading  to  the  destination  [14]. 

A  more  sophisticated  option  would  be  to  actively  map  the  operating  environment.  MONTe 
would  input  navigational  and  sensor  data  to  create  a  potential  map  that  assigns  strengths  to  the 
terrain  it  encounters.  An  algorithm  could  then  be  developed  to  determine  the  optimal  path  using 
Lagrangian  or  energy  conservation  techniques.  This  method  could  utilize  MONTe’s  terrain 
capabilities  to  drive  over  rough  terrain  if  necessary  as  opposed  to  avoiding  all  terrain  [14]. 

5.4  Remote  Operation 

One  of  the  primary  issues  currently  confronting  the  design  is  the  rudimentary  user  interface. 
While  effective  for  initial  testing  of  the  operational  program,  more  sophisticated  testing  will 
require  a  more  sophisticated  interface.  Such  a  program  would  need  to  be  able  to  load  way- 
point  routes,  read  sensor  data,  allow  for  remote  control  of  MONTe,  and  a  host  of  other  useful 
functions. 

The  legacy  NPS  GUI  displays:  1)  positional  data,  2)  a  location  chart,  3)  manual  control  panel, 
4)  waypoint  routes.  Additional  features  should  include  system  status  metrics  for  the  power  bus, 
environment,  and  other  data  of  interest.  Graphical  displays  could  be  expanded  to  include  using 
of  Google  Earth™  to  make  the  program  suitable  for  general  use.  A  display  function  for  the 
potential  map  (filtered  and  unfiltered)  would  be  useful  to  monitor  the  effectiveness  of  MONTe’s 
mapping  process. 
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APPENDIX  A: 

Simulation  Tail  Control  Code 


A.l  MATLAB  Control  Algorithm 


This  controls  MONTe’s  tail  and  is  used  in  conjunction  with  Working  Model  2D  and  MATLAB. 


% - 

%  MONTe  TAIL  CONTROL 
%  Version  2.0 
%  Written  by  Steven  Halle 

%  Thesis:  Design  and  Implementation  of  a  Semi— Autonomous  Surf— Zone  Robot 
%  using  Advanced  Sensors  and  a  Common  Robot  Operating  System 
%  30MAY2011 

% - 

%  Details  for  interfacing  WM2D  with  MATLAB  are  found  in  WM2D  user  guide 


function  tail_spd  =  TAIL2(  body _rate  ,  body_angle  ,  tail _angle  ) 
%  values  inputted  from  WM2D 
%  (pitch  rate,  pitch  angle,  tail  angle) 

%  outputs  the  tail  motor  speed 


%  declares  variables  as  global  to  hold  values  after  the  function  is  called 
global  ab  %  shorthand  for  angle  of  body  rate  ( pitch  rate ) 

global  climb  %  1  or  0  to  indicate  tail  as sitance  needed 

global  avg.rate  %  holds  the  value  for  the  averge  pitch  rate 


k=size  ( ab  ,  2 ) ; 
for  n= 1 : k— 1 

ab  ( 1  ,  k— n  +  l)=ab(l  ,  k— n); 

end 

ab(l,l)=  body_rate  ; 
avg_rate  =mean ( ab  ,  2 ) ; 


loop  to  keep  most  recent  pitch  rate  in  ab 


logs  current  body  rate  as  first  in  history 
takes  average  of  entire  ab  array 


ab_new=ab  (1  ,1:6); 

new_rate=sum(  ab.new  ) .  /  6  ;  %  local  variable  ,  average  newest  6  pitch  rates 


%  CONTROL  LOGIC 

if(avg_rate  >  4)  &&  (new.rate  <  1) 
climb  =  1 ; 
tail.spd  =60; 

elseif  (body.angle  >  5)  &&  (climb==l) 
tail.spd  =60; 

e  I  s  e  i  f  (  body  .angle  <  5.0)  &&  (tail.angl 
tail_spd=  — 60; 
climb  =0; 

else 

tail.spd  =0; 


%  presents  a  stall  climb  situation 
%  sets  climb  mode 
%  actuautes  the  tail 
%  continue s  tail  until  robot  levels 

>  145.0)  %  stows  the  tail 
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end 


A.2  MATLAB  Initialization 

This  initializes  MATLAB  in  order  execute  Appendix  A.l. 

% - 

%  MONTe  TAIL  CONTROL  INITIALIZATION 
%  Version  2.0 
%  Written  by  Steven  Halle 

%  Thesis:  Design  and  Implementation  of  a  Semi— Autonomous  Surf— Zone  Robot 
%  using  Advanced  Sensors  and  a  Common  Robot  Operating  System 
%  30MAY2011 

% - 

%  This  code  is  to  be  used  in  conjunction  with  Working  Model  2D  and  MATLAB 
%  Details  for  interfacing  WM2D  with  MATLAB  are  found  in  WM2D  user  guide 

%  Enables  MATLAB  proxy  to  interface  with  external  application 
enableservice  (  ’AutomationServer  ’  ,true  ); 

%  Initializes  global  vairables  used  in  control  program  and  sets  value  to  0 

global  climb  avg_rate  ab 

ab=zeros (1  ,100); 

avg  .rate  =0; 

climb  =0; 
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APPENDIX  B: 

MONTe  Tail  Control  Code  for  Monkey  Board 


This  code  is  an  edited  portion  of  a  Ryanmechatronics  suite  that  the  Monkey  Board  hosts  for 
autonomous  tail  control  of  MONTe. 

MODULE:  UserMode .  c 

VERSION:  1.00 

CONTAINS:  Specific  commands  and  modes  for  user  actions 

COPYRIGHT:  Ryan  Mechatronics 
Date:  Dec.  2009 

INSTALLED  CODE  FOR  AUTONOMOUS  TAIL  CONTROL 
Edited  by  Steven  Halle 

Thesis:  Design  and  Implementation  of  a  Semi— Autonomous  Surf— Zone  Robot 

using  Advanced  Sensors  and  a  Common  Robot  Operating  System 
Date:  30MAY2011 

NOTES:  This  code  is  provided  as  part  of  suite  from  Ryan  Mechantronics . 

Portions  of  the  supplied  code  were  edited  to  create  a  tail  control  algorithm. 

The  majority  of  code  pertaining  to  MONTe  resides  in  this  file.  Additional 
changes  can  be  found  in  the  errata  section  below 

Additional  Change  Section 

pwm.c  C :  \. .  .\Common\Public\PWM  3  occurrences 

Line  38  //Edit  by  Steve  Halle  — 

only  servos  5/6  will  be  used  for  MONTe 
Line  61  //Edit  by  Steve  Halle  — 

Sets  configuration  for  Servos  5/6  according  to  MONTe  design 
Line  118  //Edit  by  Steve  Halle  — 

ensures  that  servo  does  not  get  passed  value  outside  the  range  of  servo 

pwm.uplink.c  C  :\. .  .\Common\P  ublic\PWM  5  occurrences 

Line  15  static  int  bRC _U plink .Activ e  -  FALSE;  //Edit  by  Steve  Halle  — 
unsure  if  needed  to  invoke  uplink 
Line  49  //Edit  by  Steve  Halle  — 

changed  the  function  call  in  order  to  include  input  channel  parameter 
Line  52  //Edit  by  Steve  Halle  — 

removed  previous  argument  type  ,  must  now  pass  SERVO  .IN  _7  ,  etc 
Line  100  //Edit  by  Steve  Halle  — 

removed  previous  argument  type  ,  must  now  pass  SERVO JN _7  ,  etc 
Line  107  //Edit  by  Steve  Halle  — 

removed  previous  argument  type  ,  must  now  pass  SERVO  -IN  A  ,  etc 
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control,  c  C  :\. .  .\ Platforms\Public\Generic  7  occurrences 

Entire  file  Removed  from  Project  //Edit  by  Steve  Halle  — 
removed  since  PID  not  necessary  for  servo  control 

projectconfig  .  h  Monkey _User.Sandbox.SWD  1  occurrence 
Line  137  # define  CFG.CHIMU.ORIG  //Edit  by  Steve  Halle  - 

use  autoselect  is  unknown,  else  orig  as  of  4/2011 

#include  ’’globals.h” 

#include  ”  uart  .  h” 

#include  ’’lpcUART.h” 

#include  ’’math.h” 

#include  ”  iap  .  h” 

#include  ”  string. h” 

#include  ”  u ti  1  .  h” 

#include  ’’system.h” 

#include  ’’main.h” 

#  i  n  c  1  u  d  e  ”  CommOutput .  h  ” 

#include  ’’UserMode.h” 

#include  ’’events.h” 

#include  ” adc  .  h” 

#include  ”spi.h” 

#include  ” gps  .  h” 

#include  ’’stdio.h” 

#include  ”  navigation  .h” 

#include  ’’guidance.h” 

#include  ”  control. h” 

#include  ”sd_logger.h” 

# include  ”pwm.h” 

#include  ”pwm_uplink  .  h” 

#include  ”  Private\User_Library_Functions\UserFunctions  .h” 

//# include  ”  . .  \  . .  \  . .  \  . .  \  Common\  Public  \FFT\  dsplib  .testbench  _fft  _ main  .  h 

#ifdef  CFG.INSFILTER 

#include  ” i n s _ f i 1 t e r . h” 

#endif 

#ifdef  CFG.BMP085 
#include  ”bmp085.h” 

#endif 


unsigned  char  user_mode  =  0;  //  State  machine  status 

//  Nav  and  guidance  variables 
NAVIGATE  nav.sol  ; 

GUIDANCE  guide.sol  ; 


//Edit  by  Steve  Halle  — 

//  need  add  pwm.h  since  control  ()  removed 

//Edit  by  Steve  Halle  — 

//  allows  pass  thru  PWM  signals 
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//Edit  by  Steve  Halle 

//Global  Variables  declared  outside  user  function  loops 


float 

body.angle  ; 

//  This 

hold 

value  for 

the 

angle  of 

the  pelican  case  of 

the 

robot 

float 

b_a_prev  ; 

//Body 

Angle 

previous 

float 

body.rate  ; 

//  This 

holds 

value  for 

the 

rate  at 

which  the  body  .angle 

is 

changing 

float 

avg .body .rate  ; 

//  This 

holds 

value  for 

the 

average 

rate  of  body  change 

float 

b .r.history  [40] ; 

//  This 

holds 

a  history 

Of 

the  last 

40  body  .rate 

/ /  (2  seconds  worth  if  executed 

every  50ms) 

float 

tail  .output  ; 

//  This 

holds 

value  for 

the 

desired 

tail  position 

int 

climb  ; 

//  This 

holds 

status  fo 

r  the  need  fo 

r  tail  to  take  climbing 

action 

void  User_Init(  void) 

{ 

T.ed  Off  (T.F.D  RF.D  ) ; 

//Edit  by  Steve  Halle  —  below  code  is  copied  from  control,  c  to  eliminate  autopilot 
// - 

int  i  ; 

//  IN  IT  PWM  base 

//Change  this  to  PWM JOMSEC .BASE  for  100  Hz  updates  to  servos 
PWM.Init  ( PWM_20MSEC_B ASE ) ; 

//  Initialize  outputs 
Servo.Config  ( ) ; 

//The  below  may  be  moved. 

//Servo  values  should  be  updated  to  real  initial  values  before  PWM  is  started 
//to  make  sure  no  startup  glitch  occurs 

//Start  servos.  Future  Servo. Set  calls  will  just  update  value  for  a  particular  channel 
//or  a  general  update  on  all  of  them 

PWM_Start  ( ) ; 


// - end  EDIT  by  Steve  Halle  - 

} 

void  User.Main  (  void  ) 

{ 

//Main  loop 

//  Note  that  this  is  the  time  /  tasking  loop. 

//There  are  protected  areas  in  this  section  you  should  leave  alone. 

//  State  machine  for  task  processing  by  user  is  in  user  .process  ( ) 

// 

/ /  1NIT  Guidance,  Nav  and  Control  Modules  (including  PWM’s) 

//NOTE:  Waypoint  has  been  commented  out  until  we  get  IAP  going  on  Cortex 

W aypoint_Init  ( ) ; 

Navigate_Init  () ; 

//Edit  by  Steve  Halle  —  removed  control  .init  (  )  and  Guidance  .Init  ( )  since  autopilot  not  used 
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// - 

//  Guidance  .Init(& guide  .sol ) ; 

/  /  C  ontrol -Init  (  ) ; 

// - end  EDIT - 

# i f  defined  CFG.SDCARD 

bLoggerOK  =  sd_init();  //Init  SD  card  logger  if  enabled 
#endif 

gThruputCnts  =  0; 

User_Init  (); 


//Start  processing  loop 

while  (1) 

{ 

gThruputCnts ++; 

// Pseudo— tasking  occurs  here 
if  ( time.  1  ms.flag )  //  lms  (1000  Hz)  tasks 
{ 

/* - */ 

/*  BEGIN  PROTECTED  —  DO  NOT  REMOVE  —  UNIT  WILL  NOT  OPERATE  CORRECTLY  */ 

//Handle  serial  port  receipt 
Main.SerialPort.Process  (); 

//Handles  double  buffering  of  input  from  serial  corns 

//Parse  now  that  the  interrupts  seem  over  (RX  FIFO  has  been  emptied ) 
SSP1  _Parse_CHIMU  () ; 

//Parse  GPS 
GPS.Parse  ( ) ; 

//Process  uplinked  waypoints  —  Event  handler  will  indicate  if  there 
//  is  a  complete  set  waiting  for  you 
Waypoint.Process  (); 

//INS  filter  processing 
#ifdef  CFG.INSFILTER 
INS.Filter.Process  (); 

#endif 

#ifdef  CFG.BMP085 

B  aro.Process  ( 1 000) ;  //Updates  pressure  sensor  at  a  1  second  rate. 

//Maximum  update  rate  is  about  40  msec 

#endif 

/*  END  PROTECTED - */ 

/* - */ 

User.Process  () ;  //Handles  user  processing  (like  mode  switches  ,  etc ) 

time_  1  ms_flag  =  0 \// Clear  time  tick 

} 

if  ( time_5ms_flag )  //5ms  (200  Hz)  tasks 
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//No  Tasks 

time_5ms_flag  =  0 \ // Clear  time  tick 


f  (  time _  1  Oms -flag  )  //10ms  (100  Hz)  tasks 
//No  Tasks 

time.l  Oms.flag  =  0 ; //Clear  time  tick 


f  ( time_20ms -flag  )  / / 20  ms  (50Hz)  tasks 

/* - */ 

/*  BEGIN  PROTECTED  —  DO  NOT  REMOVE  —  UNIT  WILL  NOT  OPERATE  CORRECTLY  */ 

//  C  ontrol  .P  roce  s  s  (&  guide  .s  ol  ) ; 

//Handle  control  functions  (servos)  at  50Hz 
//Edit  by  Steve  Halle 

//removed  as  a  test  to  eliminate 

//control  functions  for  built  in  autopilot 

#  i  f  d  e  f  CFG_USE_MONKEY_TELEMETRY 
TX_Com_Process  ( ) ;  //Handles  output  messages 
#endif 

/*  END  PROTECTED - 

/* - 

time _20ms -flag  =  0 ;// Clear  time  tick 


*/ 

*/ 


f  ( time_  1 00ms_flag  )  //100ms  (10  Hz)  tasks 

/* - */ 

/*  BEGIN  PROTECTED  —  DO  NOT  REMOVE  —  UNIT  WILL  NOT  OPERATE  CORRECTLY  */ 

ADC_Process  () ;  //  10  Hz,  start  a  burst  ADC  read.  Global  gADC  holds  result. 
gOK_TO_SEND  =  TRUE;  //DEBUG  —  Spits  CHIMU  returning  messages  back 

/*  END  PROTECTED - */ 

/* - */ 

//Below  is  for  special  output  on  UART  1 
//  U ser _NME A  .Output  (  ) ; 

//Below  is  for  SD  card  logging  of  standard  data  set  if  card  is  present 
//Called  at  10  Hz,  but  only  writes  to  disk  after  10  entries 
# if  defined  CFG_SDCARD 
SD_StandardLogging  (TRUE) ; 

#endif 


time.  1 00ms_flag  =  0;  //Clear  time  tick 
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} 


if  ( time.l  OOOms.flag )  //1000ms  (1  Hz)  tasks 

{ 

System_Check_CPU  ( ) ;  //Checks  CPU  load  and  puts  it  into  Monkey  output  message  as  needed 
/ /  Waypoint _demo  (  ) ; 

//Edit  by  Steve  Halle  —  passes  GPS  posit  from  spare  port  in  NMEA  output  every  1  sec 
User_NMEA_Output  ( ) ; 

time.l  OOOms.flag  =  0;  //Clear  time  tick 

} 

}//Loop  back  around 

} 


void  User_Process ( void ) 

{ 

//This  is  where  mode  specific  actions  should  happen. 
//  It  is  where  most  of  your  decision  making  occurs 

static  unsigned  long  lasttime  =  0; 
static  unsigned  long  elapsed_time  =  0; 
unsigned  long  dt_msec  =  0; 

dt_msec  =  getTimeCounts  ()  —  lasttime; 
lasttime  =  getTimeCounts  () ; 
elapsed_time  +=  dt_msec  ; 


User_Event_Process  ( ) ;  //Go  check  for  events  that  may  have  occurred  (user  event  messages) 

switch  (user.mode) 

{ 

case (0) : 

//  First  two  cases  are  for  testing  and  debugging  purposes 

// Example  mode  0 

if  (  elapsed.time  >  1000) 

{ 

uartOPuts  ( ’’Here  I  am\r”); 

Led_Flash  (LED_RED ,  1 ) ; 
user_mode  ++; 
elapsed_time  =  0; 

} 

break ; 
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case ( 1 ) : 


// Example  mode  1 

if  (  elapsed.time  >  1000) 

{ 

Led.Flash  ( LED.RED  ,  2) ; 
user.mode  ++; 
elapsed.time  =  0; 

//  servo  .out  [SERVO. RIGHT _6  ] .  value  =  110;  //for  debugging  servo  output 

} 


break  ; 

case (2) : 

//Example  mode  2 


i f  (  elapsed.time  >  200) 

{ 


// loop  executed  every  200 
/  /  IMPORTANT :  calculations 


msec 

below  use  this  time  step 


int  i  ; 

float  tot_b_r=0; 

for  ( i  =  40;  i  >0;  i  — ) 

{ 

b.r.history  [i— l]=b_r_history  [i  —  2]; 

//performs  a  shift  in  the  history  array 
/ /—  b  _r  .history  [0]  remains  value  zero 

} 

for  ( i  =  1 ;  i  <40;  i  ++) 

{ 

tot_b_r  =  tot_b_r  +  b _r_hi story [  i  ] ; 

//  calculate s  the  sum  of  all  the 
//  b  _r -history  elements 

} 

avg_body_rate  =  tot_b_r/39; 

// calculate s  the  average  body  rate  based  on  the 
//last  39  array  elements  (  b  _r  .history  [0]  =  0) 

b_a_prev  =  body_angle  ; 

//assigns  the  previous  body-angle 
body.angle  =  gAttitude  .  euler  .  theta  ; 

//sets  body  angle  to  current  pitch  angle  of 
//Monkey  (in  units  radians) 
b _r _h i s tor y [0]  =  (body.angle  —  b .a.pre v ) /0 . 2 0 ; 

// calculate s  rate  based  on  50ms  calc  steps 


if  (  servo.in  [0] .  ro.msec  >  1.5) 

// s e  rv o -in  [  1  ]  matches  SERVO-INPUT-2 

{ 

LecLOn  (LED_RED ) ; 

//RED  LED  on  Monkey  means  board  is  in  manual  control 

PWM.Uplink.Passthru  (  SERVO.INPUT.2  ,  SERVO.RIGHT.6  ,  FALSE  ); 
//input  channel,  output,  failsafe  —  see  prototype 

PWM.Uplink.Passthru  (  SERVO.INPUT.3  ,  SERVO.LEFT.5  ,  FALSE  ); 
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//input  channel,  output,  failsafe  —  see  prototype 

} 

else 

{ 

//Autonomous  Mode 
//  TAIL  LOGIC 
Led.Off  (LED_RED) ; 

if  ((  avg.body  .rate  >  0.045)  &&  (  b  _r  _h i  s  t o r y  [0]  <  0.035)) 

{ 

//sense  high  center  condition 

climb  =  1 ; 

servo.out  [ SERVO_RIGHT_6  ] .  value  =  225; 
servo.out  [SERVO.LEFT.5  ].  value  =  225; 

Servos.Update.All  (); 

} 

else  if  ((  body  .angle  >  0.09)  &&  (climb==l)) 

{ 

// continue  to  hold  tail  until  climb  complete 
servo.out  [ SERVO_RIGHT_6 ] .  value  =  225; 
servo.out  [SERVO_LEFT_5  ].  value  =  225; 

Servos.Update.All  (); 

} 

else  //(  body  .angle  <  0.09) 

{ 

//return  to  neutral  position 
climb  =0 ; 

servo.out  [SERVO_RIGHT_6] .  value  =  115; 
servo.out  [SERVO.LEFT.5  ].  value  =  115; 

Servos.Update.All  (); 

} 

} 


} 


break ; 

//  Edit  by  Steve  Halle 


elapsed.time 


0; 


} 

} 


int  User_Event_Process  (  void  ) 

{ 

User_Uplink_Msg  cmdmsg ; 

switch  (  Event_Retrieve  (  &gEvent)) 

{ 

case  —1: 

return  (  —  1); 
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break  ; 
case  1: 

//Event  has  been  loaded  into  global,  now  figure  out  what  to  do  with  it 
if  ( gEvent .  id  !=  EVT_UPLINK _MSG )  return  (-1); 

//This  ignores  other  uplink  messages  , 

//like  waypoints  ,  etc...  that  are  handled  by  the  main,  common  processing 
memmove(  &cmdmsg  ,  &gEvent .  payload  ,  gEvent .  length  ) ; 

//  If  a  mode  switch,  take  it  and  leave 
if  (user.mode  !=  cmdmsg.mode) 

{ 

user.mode  =  cmdmsg.mode; 
return  (1); 
break ; 

} 

// 

//For  example  message,  we  have  a  command  word  that 
//indicates  what  command  has  been  sent: 

// 

switch  ( cmdmsg  .  action) 

{ 

case  USER_ACTION_REQUEST_STATUS : 

User_Status_Output  (); 

break ; 

case  USER_ACTION_ABORT : 

break ; 

case  USER_ACTION.CMD  .ANGLES : 
g u ide _s o  1  .  a 1 1  _de s  .  eu ler  .  phi  = 
guide.sol  .  att.des  .  euler  .  theta 
g  u  ide  _s  o  1  .  a  1 1  _de  s  .  eu  ler  .  p  s  i  = 
break ; 

return  (  1 ) ; 
break ; 
default  ; 

return  ( —  1); 
break ; 

} 

} 

} 


unsigned  char  User.Status.Output  (  void  ) 

{ 

//  This  is  a  message  wrapper  for  special  user  messages . 
//Normal  telemetry  contains  most  items  of  interest 
//  This  message  wraps  output  in  the  user  message  format  OxAA 
// 

int  index  =  0; 

short  int  sint.tmp  =  0; 

float  ftmp  =  0.0; 


cmdmsg.  phi_desired  ; 
cmdmsg.  theta_desired  ; 
cmdmsg .  psi_desired  ; 
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int  i  =  0; 

i  f  (  gUserBytesOut  .  isbusy  ==  TRUE)  return  (FALSE); 

//Buffer  hasn’t  been  transmitted  yet,  don’t  respond  yet 
//  Example  output  message  is: 

//  Modes  /  status 
//  Timer  /  counters 
//  Example  float  data 

// 

//Modes 

gUserBytesOut  .  payload  [  index  ]=  0x01;  index++; 

//User  message  ID  —  chose  0x01  for  no  good  reason 
gUserBytesOut .  payload  [  index  ]=  0x00;  index++; 

//User  message  length  ( overwrite  at  bottom  once  index  is  summed  up) 
gUserBytesOut  .  payload  [  index  ]=  user_mode  ;  index++; 

//Local  state  machine 
//  Timers  /  counters 

memmove(&gUserBytesOut .  payload  [  index  ]  , 

&gTime_Msec  ,  sizeof  (  unsigned  long)); 

index  +=  sizeof  ( unsigned  long); 

//  System  running  time  in  milliseconds 
memmove(&gUserBytesOut .  payload  [  index  ]  , 

&gThruputHz  ,  sizeof  (  unsigned  long)); 

index  +=  sizeof  ( unsigned  long); 

//  Thruput 

//  Example  float  data 
ftmp  -  (99.0); 

memmove(&gUserBytesOut .  payload  [  index  ]  ,  &ftmp  , 
sizeof  (  float  )); 

index  +=  sizeof  (  float  ) ; 

//  Done  now  with  populating 

//  Now,  replace  the  length  byte  with  our  total  index  to  help  decoding 
gUserBytesOut  .  payload  [  1  ]=  index  ;  //No  bumping  index  here,  we  are  just 
gUserBytesOut  .  length  =  index; 

//  Now,  the  global  user  bytes  are  all  setup  and  ready  to  go 
//  Flag  it  as  full  ,  then  send  it  all  out  using  the  message  OxAA  user 
//  When  it  is  gone,  the  flag  will  be  cleared 
gUserBytesOut .  isbusy  =  TRUE; 

Tx .Com _Add .Special .Message  ( MSGOut_0xB0_User_l  ) ; 
return  (TRUE); 


unsigned  char  User_NMEA_Output  (  void  ) 

{ 

// Example  output  of  various  data  in  NMEA  format  on  the  spare  UART 

//For  details  of  GPS  structure  see  globals.h 

int  index  =  0; 

int  i  =  0; 

char  stemp  [128]; 


by  a  ground  station 
replacing  a  value 

bytes  wrapper 
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char  pOutString  [  1  96] ; 
unsigned  char  pFieldXSUM  =  0; 


//  Send  Header  first 
sprintf  (  pOutString 
//  Add  other  data 
//GPS  first 

sprintf  (stemp  ,  ”%01i  ,%081d  ,%081d  ,%03  Id  ,%041d  ,%04  Id  ,%041d,%lX  ,  ( int  )gGPS.  s  at  s  tracked  , 

( long  )  ( gGPS  .  latitude  *10000),  (long)(  gGPS  .  longitude*10000), 
(long  )  (gGPS  .  altitude  )  .(long  )  (gGPS  .  velN  *  10) , 

( long  )  ( gGPS  .  velE  *10),  ( long  )( gGPS  .  velD  *  1 0) ,  ( long  )( gGPS  .TOW) ) 

s  t  r  c  a  t  (  pOut  String  , stemp); 

sprintf  (stemp  ,  ”%041d  ,%041d  ,%041d  ,%041d  ,%041d  ,%041d  ,%041d  ,%041d  ,%041d  . 

( long  )  (  gAttitude  .  euler  .  phi*100) , 

( long  X  gAttitude  .  euler  .  theta*100), 

( long  X  gAttitude  .  euler  .  psi  *  100) . 

( long  X  gSensor.  rate  [()]*  100)  , 

( long  X  gSensor.  rate  [1  ]*  100)  , 

(longX  gSensor.  rate  [2]*  100)  , 

(long  )(  gSensor  .  acc  [()]*  100)  , 

(long  X  gSensor  .  acc  [1]*  100)  , 

(longX  gSensor.  acc[2]*100)); 
s  t  r  c  at  (  pOut  String  , stemp); 

#ifdef  CFG.IN SFILTER 

sprintf  (  stemp  ,  ”%081d  ,%08  Id  ,%03  Id  ”  , 

( long  )(  gINS  .latitude  *10000), 

( long  X  gINS  .  longitude*10000), 

( long  )  (  gINS  .  altitude  ) ) ; 
s  t  r  c  a  t  (  pOut  String  , stemp); 

#endif 

//OK,  now  we  have  the  whole  string.  Time  to  find  the  checksum 
for  (i=l;  i<s  trie  n  (  pOutString  );  i ++) 

//NOTICE:  Starting  at  1,  because  xsum  doesn’t  use  $  in  front 

{ 

pFieldXSUM  "=  pOutString  [  i  ] ; 

} 

//Tack  it  on  the  end 
sprintf  (  stemp  ,  ”*%02x”  ,  pFieldXSUM  ) ; 
s  trc  at  (  pOutString  , stemp); 

//Output  the  string 
uartlPuts  (pOutString  ); 

//Output  the  <CRxLF> 
uartlPutch  (OxOD ) ; 
uartlPutch  (OxOA ) ; 

return  (TRUE); 

} 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 
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APPENDIX  C: 
ROS  Code 


All  electronic  versions  of  this  code  will  be  incorporated  into  the  upcoming  NPS  ROS  Stack. 


C.l  Navigation  Node 


Following  code  performs  navigation  for  MONTe. 


Title:  MONTe_Nav  0.0  (ROS  Node) 

Author:  Jason  Hickle 


Purpose:  Node  that  allows  MONTe  to  navigate  through  the  world. 

Handles  the  following  functions  : 

1)  Monitors  Command-Flags  to  determine  behavior . 

2)  Pulls  current  position  data  (GPS  and  heading ) 

3)  Gets  current  waypoint 

4)  Computes  desired  heading  and  range  to  waypoint . 

5)  Computes  plant  commands  using  current  and  desired  hdg . 

6)  Updates  Command-Flags  and  Plant-Control 

Use:  Communication  protocol  handled  via  ROS  messaging .  Launch  program  as 

part  of  roslaunch. 

Runs  at  loop  rate  of  4  Hz. 


ROS  Notes: 

Name- 

Publications— 

Sub  scriptions  — 

Messages  — 

Services  — 


”  MONTe  -Navigation  ” 

”  Plant -Command -T” 

”  Command-Flags -T” 

”  Command-Flags -T” 

”  Nav -Data-T” 

”  Current  -W  ay  point  -T 

Command-Flags .  msg 
Plant -Command .  msg 
Waypoint .  msg 

None 


Version  History: 

-  Version  0.0  - 

Mar  21st  ,  2011 
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LT  Jason  Hickle 


Enable  Manual  Control  mode  in  main  control  loop. 


/* 

#include 

#include 

#include 

#include 

#include 

#include 

#includc 


Libraries 
<ros / ros . h> 

<math  .  h> 

”std_msgs/String  .h" 
”MONTe/  Plant.Command  .  h 
”MONTe/  Waypoint .  h” 
"MONTe/  Command.Flags  .  h 
"MONTe/  Nav_Data  .  h” 


*/ 


//  Message  format  ( .  msg ) 
//  Message  format  (.msg) 
//  Message  format  (.msg) 
//  Message  format  (.msg) 


#include  <sstream> 


/* 

D  efi  nes 

*/ 

const 

double  PI 

=  3.14159265; 

const 

const 

unsigned 

unsigned 

char  STOP.R  =  190; 

char  STOP_L  =  64; 

// 

// 

const 

const 

unsigned 

unsigned 

char  R.CORRECT  =  1; 

char  L.CORRECT  =  3; 

// 

const 

const 

double  RANGE.THRESH  =  2.5; 

float  HDG_ERROR_THRESH  =  3.0; 

Right  motor  stop 
Left  motor  stop 

Correction  factor  to  calibrate  forward  speed 


const  char 
const  char 


FWD  SPFF.D  =  30; 
MAX_TURN  _DIFF  = 


10; 


/* 

/ 

/ 

typedef  struct 


CF  holds  all  command  flags.  Expand  as  necessary .  Ensure  publishing 
utilizes  default  values  for  any  flag  unchanged  to  ensure  flags  are 
not  being  changed  unecessarily . 


/ 

/ 

*/ 


{ 


}CF ; 


bool  autonav ; 
char  mode ; 
bool  route  ; 


// D efi ne  flags  structure 


/*  WPJ3  stores  all  data  necessary  to  navigate  to,  and  determine 

/  behavior  mode  for  a  route. 

typedef  struct 

{ 

double  I  at  ; 
double  Ion  ; 
char  action  ; 


/ 

*/ 


}WP; 


//Define  waypoint  structure 


/* 


NAVDATA  is  for  current  positional  data 


*/ 


66 


typedef  struct 

{ 

double  lat  ; 
double  Ion  ; 
double  heading  ; 
}NAVDATA; 


//Define  Current  Position  structure 


/*  Global  Variables  */ 

WP  Current.WP  ; 

CF  Cmd.Flags  ; 

NAVDATA  Current_Nav_Data  ; 

double  K_P_coefficient  ;  //  Proportional  constant  for  PID  control 

float  right_command  ,  left_command  ; 

/*  Functional  Prototypes  */ 

void  Command.FlagsCallback  (  const  MONTe :  :  Command_FlagsConstPtr&  flags); 
void  WaypointCallback  (  const  MONTe:  :  WaypointConstPtr&  way.pt); 
void  Nav.DataCallback  (  const  MONTe ::  Nav_DataConstPtr&  nav.dat); 
int  Navigate  ( ) ; 

int  Plant.Compensator  ( double  range,  float  desired.hdg  ,  float  current.hdg  ) ; 

int  main(int  argc  ,  char  **argv) 

{ 

/ *  ROS  Initializations  */ 

ros  :  :  i  n  i  t  (  argc  ,  argv  ,  "MONTe.Navigation”  ) ;  //  Set  up  ROS  node 

ros  :  :  NodeHandle  n;  //  set  up  handle  for  this  node 

//  Set  up  all  publishers  for  node 

ros  ::  Publisher  plt.cmd.pub  =  n  .  advertise  <MONTe:  :  Plant-Command  >(”Plant_Command_T”  ,  1) 

ros  ::  Publisher  cmd.flg.pub  =  n  .  advertise  <MONTe:  :  Command-Flags >(”Command_Flags_T”  ,  1) 

//  Set  up  all  subscriptions  for  node 

ros  ::  Subscriber  sub.cf  =  n  .  subscribe  ( ”Command_Flags_T”  ,  1,  Command.FlagsCallback  ) ; 
ros  ::  Subscriber  sub.wp  =  n  .  subscribe  (”  Current.Waypoint.T”  ,  1,  WaypointCallback); 
ros  ::  Subscriber  sub.nav  =  n .  subscribe  (”Nav_Data_T”  ,  1,  Nav.DataCallback); 

ros::  Rate  loop  .rate  (4) ;  //  Sets  4hz  cycle  for  main  loop 

//  Set  up  message  handles  for  communicating  with  topics. 

MONTe ::  Command-Flags  flags  ,  pub.flags  ; 

MONTe ::  Waypoint  way.pt; 

MONTe:  :  Plant-Command  cmd; 

MONTe ::  Nav  .Data  nav.dat; 
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in  t  nav_val  =  0; 

CmcLFlags  .  autonav  =  0; 

/*  Calculate  Kp  coefficient  for  turning.  Define  MAX-TURN  above  */ 

K_P_coefficient  =  pow  ((((  double  )  MAX_TURN_DIFF )  /  8  0 . 0 )  ,  (1. 0/3.0)); 

/*  End  ROS  Initializations  *  / 

while  (  ros  :  :  ok  ( ) ) 

{ 

ros  :  :  spinOnce  ( ) ;  //  Callback  to  all  active  topics 

/*  Begin  Navigation  */ 

if  (  CmcLFlags  .  autonav  ==  1) 

{ 

nav_val  =  Navigate  (); 

if(nav_val  ==  1)  //  Waypoint  reached,  inform  and  stop  MONTe 

{ 

pub_flags.auto.nav  =  Cmd_Flags  .  autonav  ; 
pub.flags  .  nav.mode  =  ’N’ ; 

pub.flags  .  incoming.route  =  Cmd.Flags  .  route  ; 

//  N  indicates  waypoint  reached,  need  next  waypoint 
cmd_flg_pub  .  publish  (  pub  .flags  ) ;  //  Publish  to  cmd-flags  topic 

cmd.left  =  (unsigned  char)  STOP.L ;  //  Sen  d  stop  command  to  PlantControl 
cmd.  right  =  (unsigned  char)  STOP.R ;  // while  waiting  for  next  waypoint 
plt.cmd.pub  .  publish  (cmd ) ; 

} 

else  //  Send  command  signal  to  plant  to  move  towards  waypoint 

{ 

cmd.left  =  (unsigned  char)  left.command  ; 
cmd.  right  =  (unsigned  char)  right.command ; 
plt.cmd.pub  .  publish  (cmd ) ; 

} 

}  //  End  Navigation 

loop  .rate  .  sleep  () ;  //  Sleeps  to  maintain  loop.rate 

}  //  end  while  loop 

}  //end  main 

Function  :  Command -FlagsCallback 

Description:  Pulls  waypoint  from  the  Command-Flags -T  topic 

Parameter:  1)  Pointer  to  ROS  message  from  topic  Command-Flags -T 

Return  Value:  None 

void  Command.FlagsCallback  (  const  MONTe ::  Command_FlagsConstPtr&  flags) 

{ 
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CmcLFlags  .  autonav  =  flags— >  auto_nav  ; 

Cmd.Flags  .  mode  =  flags— >  nav.mode  ; 

Cmd.Flags  .  route  =  flags— >  incoming-route; 

}  //  end  callback 

Function:  WaypointCallback 

Description:  Pulls  waypoint  from  the  Current  .Way  point  _T  topic 

Parameter:  1)  Pointer  to  ROS  message  from  topic  Current  .Waypoint  _T 

Return  Value:  None 

void  WaypointCallback  (  const  MONTe :  :  WaypointConstPtr&  way_pt) 

{ 

Current_WP  .  1  at  =  way_pt— >latitude  ; 

Current_WP  .  Ion  =  way_pt— >longitude  ; 

}  //  end  callback 

Function  :  Navigation  .DataCallback 

Description:  Pulls  waypoint  from  the  Current  .Waypoint  _T  topic 

Parameter:  1)  Pointer  to  ROS  message  from  topic  Current  .Waypoint  _T 

Return  Value:  None 

void  Nav_DataCallback  (  const  MONTe ::  Nav_DataConstPtr&  nav_dat) 

{ 

Current_Nav_Data  .  1  at  =  nav_dat— >latitude  ; 

Current_Nav_Data  .  Ion  =  nav_dat— >longitude  ; 

Current_Nav_Data  .  heading  =  nav_dat— >heading  ; 

}  //  end  callback 

Function:  Navigate 

Description:  Navigate  MONTe.  Pulls  current  posit  and  waypoint .  Determines 

range  and  heading  to  de sitiantion  .  Calls  plant  control  function  to 
produces  plant  command. 

Parameter :  None 

Return  Value:  0  —  Success 

int  Navigate  () 

{ 

static  double  lat  ,  Ion,  *lat_ptr,  *lon_ptr;  //  Current  position 

static  double  wlat  ,  wlon  ,  *wlat_ptr,  *wlon_ptr;  //  Waypoint  position 
static  double  lat.diff  ,  lon.diff  ,  *  lat_diff_ptr  ,  * lon.diff _ptr ; 
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//  variables  to  calculate  differences  in  latitude  &  longitude 
static  double  rng  ,  *rng_ptr;  //Range  (in  yards ) 

static  float  cur.hdg  ,  new.hdg  ,  *  cur _hdg _ptr  ,  *  new_hdg_ptr  ; 

/*  Pointer  initializations  *  / 

lat.ptr  -  &lat ; 

Ion _ptr  -  &lon  ; 
wlat.ptr  =  &wlat  ; 
wlon.ptr  =  &wlon  ; 
rng _ptr  -  &rng ; 
lat_diff_ptr  =  &lat_diff; 
lon_diff_ptr  =  &lon_diff; 
cur_hdg_ptr  =  &cur_hdg  ; 
new_hdg_ptr  =  &new_hdg  ; 

/*  Update  variables  */ 

*lat_ptr  =  Current_Nav_Data  .  1  at  ; 

*lon_ptr  =  Current_Nav_Data  .  Ion  ; 

*wlat_ptr  -  Current.WP  .  1  at  ; 

*  wlon.ptr  -  Current.WP  .  Ion  ; 

*cur_hdg_ptr  =  Current.Nav.Data  .  heading  ; 

/*  Compute  range  to  current  waypoint .  */ 

*rng_ptr  =  sqrt((((2000  *  (*  wlat.ptr ))  —  (2000  * 

(*  lat.ptr  )))*((2000  *  (*  wlat.ptr )) —(2000  *  (*  lat.ptr  ))))  + 
(((1600  *  (*  wlon.ptr )) —  (1600  *  (*  lon.ptr  )))* 
((1600  *  (*  wlon _ptr ))  —  (1600  *  (*  lon.ptr  ))))) ; 

if  (*rng_ptr  <=  RANGE.THRESH )  // When  close  enough  to  waypoint  ,  action 
{  //code  takes  effect  and  next  waypoint  is  loaded 

return  1; 

} 


//  3600  converts  lat_diff  and  lon_diff  to  decimal  seconds  for  accuracy 

*  lat.diff  _ptr  =  3600  *  ((*  wlat.ptr)— (*  lat.ptr  )) ; 

*  Ion  _diff  _ptr  =  3600  *  ((*lon_ptr)  —  (*  wlon.ptr  ) ) ; 

//  Compute  new Jidg  using  the  differences  in  lat/long 
*new_hdg_ptr  =  atan2  (*  Ion _diff  _ptr  ,  *  1  a t  _d i f  f  _p  t r  )*  1  80/ PI ; 

//  Convert  quadrant  III/IV  degrees  to  180—360 
if  (*  new.hdg.ptr  <  0.0) 

*new_hdg_ptr  +=  360.0; 

Plant.Compensator  (*  rng _ptr  ,  *  new.hdg.ptr  ,  *  cur  .hdg.ptr  ) ; 
return  0; 

}  //  end  Navigate 

Function:  Plant.Compensator 
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Description  :  PID  control  for  MONTe.  P  &  D  control  turning  rate.  I 

maintains  forward  speed.  Checks  to  keep  wheels  moving  forward  so  no 
tank  turns  happen.  Also,  limits  maximum  turn  rate  to  avoid  spinning  . 

Parameter:  1)  Range  to  waypoint 

2)  Desired  heading  to  waypoint 

3)  Current  heading  of  MONTe 

Return  Value:  None 

int  Plant-Compensator  ( double  range,  float  desired_hdg  ,  float  current.hdg) 

{ 

float  hdg_error; 
unsigned  char  k_p_left  =  0; 
unsigned  char  k_d_left  =  0; 
unsigned  char  k_p_right  =  0; 
unsigned  char  k_d_right  =  0; 
unsigned  char  k_i  =  0; 

hdg.error  =  desired.hdg  —  current.hdg  ; 

if  (hdg.error  >  180.0) 

hdg.error  — =  3  60.0; 
else  if  (hdg.error  <=  —180) 
hdg.error  +=  360; 

/*  Proportional  &  Derivative  control  with  gain  scheduling  */ 

/*  Max  turn  if  outside  of  +\—  80  degrees  */ 

if  ((  hdg_error  <  80.0)  ||  (hdg_error  >  80.0)) 

{ 

k_p_left  =  — MAX_TURN_DIFF  *  (char)  hdg_error  /  (char)  fabs  (  hdg_error  ) ; 
k_p_right  =  MAX_TURN_DIFF  *  (char)  hdg_error  /  (char)  fabs  ( hdg_error  ) ; 
}  else 

{  //  Proportional  gains  for  turning  ,  in  form  of  k_p  =  A*x*3 

k_p_left  =  (char)  (— K_P_coefficient  *  pow(  hdg.error  ,  3.0)); 

k_p_right  =  (char)  (  K_P_coefficient  *  pow(  hdg.error  ,  3.0)); 

//  Derivative  gain  for  turning 

} 

/*  Integral  control  for  maintaining  velocity  */ 

/*  Assign  speeds  and  provide  limiting  */ 

left.command  =  FWD.SPEED  +  L.CORRECT  +  k.p.left  +  k_i  +  k.d.left  ; 
right_command  =  FWD.SPEED  +  R.CORRECT  +  k_p_right  +  k_i  +  k_d_right  ; 

/*  Keep  commands  inside  design  limitations  incase  of  data  corruption  */ 

if  ( left_command  >  117.0)  //  Design  speed  range  for  left  side 

left_command  =  117.0;  //  65—117  (64  stop,  127  full) 

else  if  ( left_command  <  65.0) 
left.command  =  65.0; 
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if  ( right_command  >  246.0)  //  Design  speed  range  for  right  side 

right-command  =  246.0;  //  191—246  (190  stop,  256  full) 

else  if  (right-command  <  191.0) 
right-command  =  191.0; 

return  0; 

}  //  end  Plant  .Control  _Func 


C.2  Waypoint  Processing  Node 


Following  code  processes  waypoint  routes  for  MONTe. 


Title:  MONTe  .Waypoint  .Processing  0.0  (ROS  Node ) 

Author:  Jason  Hickle 

Purpose:  Receives  ,  store  and  publishes  waypoint  data  for  use  in  autonomous 

navigation  . 

Handles  the  following  functions : 

1)  Monitor  Command-Flags  to  determine  behavior . 

2)  Receive  waypoints  from  New.Waypoint  and  store  them. 

3)  Publish  current  waypoint  for  use  in  Navigation. 

Use:  Interface  between  user  and  navigation  node.  When  CF.  route  equals 

"true,”  program  proceeds  to  receive  and  process  a  waypoint  router. 

Can  work  with  any  other  node  that  publishes  to  ”  New  .Waypoints  _T .  ” 
Receives  waypoints  (up  to  10)  and  stores  for  sending  waypoints  to  the 
Navigation  node.  Will  receive  waypoints  from  Waypoint  .Control  (user 
input  over  VNC) ,  or  via  Comms  (wireless  comms  from  base  station) 

Waits  for  nav .mode  ’N’  to  send  new  waypoint  to  navigation  mode. 
Receives  from  Command-Flags . 

Node  set  up  to  allow  future  capability  .  Possible  functions  could 
be  to  set  up  search  types  ,  additional  actions  to  perform  upon  reaching 
waypoint  ,  etc. 

Loop  rate  set  at  4  Hz.  This  synchs  with  system  loop  rate. 


ROS  Notes: 

Name- 

Publications— 

Subscriptions  — 

Messages  — 


”  MONTe. Way  point -Processing 

”  Current  .Waypoint  .T  ” 

”  Command  .Flags  .T” 

”  New  .Waypoints  _T” 

”  Command  .Flags.  T” 

Waypoint .  msg 
Command-Flags .  msg 
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Services  — 


None 


Version  History: 

-  Version  0.0  - 

May  22nd,  2011 
FT  Jason  Hickle 

Establishes  basic  functionallity  .  Receives  ,  stores  and  sends 
waypoints  based  off  of  Command  .Flags  _T 

Notes:  Further  information  can  be  found  on  ROS  Wiki  page: 

http  : / /www .  ros  .  org / wiki / 

/*  Libraries  */ 

#include  <ros/ros.h> 

#include  ”MONTe/  Waypoint .  h”  //  Message  format  (.msg) 

#include  ”MONTe/ Command.Flags  .  h”  //  Message  format  (.msg) 

/*  D  efi  nes  *  / 

//  Debugging  options  ,  uncomment  to  enable 

#define  WPJNCOMENG  //  Publish  info  to  ROS  for  incoming  waypoints 

#define  WPJSEND  //  Alert  ROS  when  a  new  waypoint  is  sent 

/*  CF  holds  all  command  flags.  Expand  as  necessary .  Ensure  publishing 

/  utilizes  default  values  for  any  flag  unchanged  to  ensure  flags  are 

/  not  being  changed  unecessarily . 

typedef  struct 

{ 

bool  autonav ; 
char  mode; 
bool  route  ; 

}CF;  //Define  flags  structure 

/*  WPJ°  stores  all  data  necessary  to  navigate  to,  and  determine 

/  behavior  mode  for  a  route. 

typedef  struct 

{ 

i  n  t  num ; 

double  lat  ; 
double  Ion  ; 
char  action  ; 

}WP_P;  //Define  waypoint  structure 

/*  Global  Variables  */ 

WP_P  New_WP,  waypoints  [  1  0] ; 

CF  Cmd.Flags  ; 


int  route_points  =  0;  //  Stores  length  of  route 


/*  Functional  Prototypes  */ 

void  Command_FlagsCallback  (  const  MONTe :  :  Command_FlagsConstPtr&  flags); 

void  WaypointCallback  (  const  MONTe ::  WaypointConstPtr&  way.pt); 

int  main(int  argc  ,  char  **argv) 

{ 

/*  ROS  Initializations  *  / 

ros  :  :  i  n  i  t  (  argc  ,  argv  ,  ’’MONTe.Waypoint.Processing”  ) ;  //  Set  up  ROS  node 

ros  : :  NodeHandle  n;  //  set  up  handle  for  this  node 
//  Set  up  all  publishers  for  node 

ros  ::  Publisher  nxt.wp.pub  =  n  .  advertise  <MONTe:  :  Waypoint  >(”  Current.Waypoint.T  ”  ,  1); 

ros  ::  Publisher  cmd.flg.pub  =  n  .  advertise  <MONTe:  :  Command.Flags  >(”Command_Flags_T”  ,  1) 

//  Set  up  all  subscriptions  for  node 

ros  :  :  S ub scriber  sub.cf  =  n  .  subscribe  ( ’’Command.Flags.T”  ,  1,  Command.FlagsCallback  ) ; 
ros  :  :  S  ub  scriber  sub.wp  =  n  .  subscribe  ( ’’New.Waypoint.T”  ,  1,  WaypointCallback); 

ros  ::  Rate  loop  .rate  (4) ;  //  Sets  4hz  cycle  for  main  loop 

//  Set  up  message  handles  for  communicating  with  topics. 

MONTe ::  Command.Flags  flags  ,  pub.flags  ;  //  subscribe  and  publiser  handles 
MONTe ::  Waypoint  new_way.pt,  next_way.pt;  //  subscribe  and  publiser  handles 

/  /  int  total -wp -counter  =  0; 

//  int  wp -entry -counter  =  0; 
int  current.wp.number  =  0; 

pub.flags  .  auto.nav  =  0;  //  Initialize  system  in  manual  control,  enroute  to 

pub.flags  .  nav.mode  =  ’A’;  //  next  waypoint  ,  and  no  new  route 
pub.flags  .  incoming-route  =  0; 
cmd.flg.pub  .  publish(pub_flags  ); 

while  (  ros  : :  ok  ( ) ) 

{ 

ros  :  :  spinOnce  ( ) ;  //  Callback  to  all  active  topics 

//  Check  for  new  route  and  fill  in  waypoints 
i f  (  Cmd.Flags  .  route  ==  1) 

{  //  Check  for  errors  and  fill  in  new  waypoint 

i f  ( ( New.WP . num  >=  0)  &&  (New.WP.num  <  10)) 

{  //  Populate  the  waypoint  queue 

waypoints  [New.WP . num ].  num  -  New.WP.num; 
waypoints  [New.WP.num] .  lat  -  New.WP.  lat; 
waypoints  [New.WP .  num  ].  Ion  -  New.WP.  Ion; 
waypoints  [New.WP.num] .  action  -  New.WP.  action  ; 
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#  i  f  d  e  f  WPJNCOMING 

ROS _INFO ( ’’New  WP:  #%d  ,  Lat-  %lf  ,  Lon-  %lf  ,  Action-  %c, 
Route  Size—  %d”  ,  waypoints  [New.WP .  num  ].  num ,  waypoints  [New _WP .  num  ].  1  at  , 
waypoints  [New.WP.  num]  .Ion,  waypoints  [New.WP.  num]  .action,  route_points); 

#endif 

} 

//  End  of  route  received  ,  place  in  ”no  new  route ” 

i  f  ( New.WP .  num  ==  (route.points  —  1)) 

{ 

pub  .flags  .  incoming.route  =  0; 
pub  .flags  .  auto  _nav  =  Cmd.Flags  .  autonav  ; 
pub  .flags  .  nav.mode  -  Cmd.Flags  .  mode  ; 
cmd.flg.pub  .  publish(pub_flags  ); 

} 

}  //  End  New  Route  if 

//  Send  waypoint  if  auto  nav  and  next  waypoint 

//  check  for  auto_nav  and  if  new  waypoint  is  needed 

i f  ((  Cmd.Flags  .  autonav  =-  1)  &&  (  Cmd.Flags  .  mode  ==  ’N’ )) 

{ //  New  waypoint  is  available 

if  ((  current.wp.number  <  route  .points  )  &&  (  current.wp.number  >=  0)) 
{  //  Send  next  waypoint 

next.way  _pt  .  1  atitude  =  waypoints  [  current.wp.number  ].  1  at  ; 
next_way.pt  .  longitude  =  waypoints  [  current.wp.number  ].  Ion  ; 
next.way  _pt  .  action  =  waypoints  [  current.wp.number  ].  action  ; 

nxt.wp.pub  .  publish  (  next_way.pt  ) ; 

#ifdef  WP.SEND 

ROS_INFO( ” Waypoint  %d  sent  to  Nav.”,  current.wp.number); 
#endif 

current.wp.number ++; 

}  else  //  Route  complete  ,  reset  waypoints 

{ 

pub_flags  .  auto.nav  =  0; 
pub.flags  .  nav.mode  -  ’A’; 

pub.flags  .  incoming.route  =  Cmd.Flags  .  route  ; 
cmd.flg.pub  .  publish  (pub.flags  ); 

#ifdef  WP.SEND 

ROS_INFO(” Route  complete,  in  Manual  Control.”); 
#endif 

current.wp.number  =  0; 

}  //  End  Send  wp/ route  complete 

} 

loop  .rate  .  sleep  () ;  //  Sleeps  to  maintain  loop-rate 

}  //  End  main  while 
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}  //end  main 


Function :  Command. FlagsCallback 

Description:  Pulls  waypoint  from  the  Command  .Flags  _T  topic 


Parameter:  1)  Pointer  to  ROS  message  from  topic  Command. Flags _T 


Return  Value:  None 

void  Command.FlagsCallback  (  const  MONTe :  :  Command_FlagsConstPtr&  flags) 

{ 

Cmd_Flags  .  autonav  =  flags  — >auto _nav  ; 

Cmd_Flags  .  mode  =  flags  — >nav_mode  ; 

Cmd_Flags  .  route  =  flags  — >incoming_route  ; 

}  //  end  callback 


Function:  WaypointCallback 


Description:  Pulls  waypoint  from  the  Current  .Way  point  _T  topic 


Parameter:  1)  Pointer  to  ROS  message  from  topic  Current  .Way  point  _T 


Return  Value:  None 

void  WaypointCallback  (  const  MONTe ::  WaypointConstPtr&  new_way_pt) 

{ 

New_WP.  lat  =  new_way_pt— >latitude  ; 

New  WP .  Ion  =  new_way_pt— >longitude  ; 

New_WP.  action  =  new_way_pt— >action  ; 

New_WP.num  =  new  way  pt— >wp  num  ; 
route_points  =  new_way_pt— >route  ; 

}  //  end  callback 


C.3  Monkey  Driver 

Following  driver  interfaces  with  Monkey  board. 

Title:  MONTe  .Monkey  .Driver  0.0  (ROS  Node ) 

Author:  Jason  Hickle 

Purpose:  Node  that  allows  MONTe  to  interface  with  CH1MU  AHRS  ("Monkey”). 

Handles  the  following  functions : 

1)  Read  in  serial  data  from  Monkey  AHRS. 

2)  Parse  data  string. 

3)  Publish  data  to  Nav.Data  topic. 
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Use : 


Driver  for  Monkey  nav  unit. 

Output  from  Monkey  needs  to  be  the  following  comma  separated  values  (CSV): 
$RM 

Satellites  Tracked 
Fix  Quality 
Latitude 
Longitude 
Heading 
Altitude 
Velocity  (N/S) 

Velocity  (E/W) 

Velocity  (D/U) 

Additional  parameters  can  be  included  by  expanding  the  parsing  function  . 

Runs  at  loop  rate  of  4  Hz.  This  is  MONTe  ’ s  program  system  loop 
rate  . 

ROS  Notes: 

Name—  ”  MONTe  -Monkey” 

Publications  —  ”  Command  .Flags  .T” 

”  Nav.Data.T” 

Subscriptions  —  None 

Messages—  Command  .Flags  .msg 

Nav  .Data .  msg 

Services—  None 

Version  History: 

-  Version  0.0  - 

Mar  21st  ,  2011 
LT  Jason  Hickle 

Established  link  with  Monkey  at  115200  Baud.  Code  in  place  to 
receive  full  data  string  (waiting  on  adjustment  of  code  on  Monkey 
board  itself).  Successfully  transmit  nav  data  to  ”  Nav  .Data.T .  ” 

Notes:  Further  information  can  be  found  on  ROS  Wiki  page: 

http  :  /  fwww .  ros  .  org  /  wiki  / 

/*  Libraries  */ 

#include  <ros/ros.h> 

#include  ’’MONTe/ Nav_Data  .  h”  //  Message  format  (.msg) 

#include  ’’MONTe/ Command-Flags  .  h”  //  Message  format  (.msg) 

#include  ”MONTe_USB_Serial_Lib  .  h” 
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/*  D  efi  nes  *  / 

//  Debugging  options  ,  uncomment  to  enable 

//# define  NAV-PRINT  //  Print  out  nav  data  and  check  for  successful 

//  parsing  of  command  string 

NMEA_DATA  is  the  data  structure  to  hold  navigational  data  received  / 

from  the  Monkey / AHRS .  Listed  in  order  of  data  receieved  via  command  / 

string.  Use  this  to  expand  MONTe’s  capabilities  for  processing  data.  */ 

typedef  struct  { 

unsigned  char  command.str  [  3  ] ; 
int  sat.track  ; 
int  fix_quality  ; 
double  latitude  ; 
double  longitude  ; 
double  heading  ; 
int  altitude  ; 
double  vel_N  ; 
double  vel_E  ; 
double  vel_D  ; 

}  NMEA_DATA ; 

/*  Global  Variables 

NMEADATA  GPS  .Buffer; 
char  nav.buffer  [254] ; 

/*  Functional  Prototypes 

int  Parse.Monkey .Data  ( ) ; 

void  Print.Nav.Data  ( ) ; 

int  main(int  argc  ,  char  **argv) 

{ 

int  usb.fd;  //  File  descriptor  for  serial  comms ,  included  for  future  use 

//  for  error  checking 

/ *  ROS  Initializations  *  / 

ros  ::  i  nit  (argc  ,  argv  ,  ”MONTe_Monkey”  ) ;  //  Set  up  ROS  node 

ros  :  :  NodeHandle  n;  //  set  up  handle  for  this  node 

//  Set  up  all  publishers  for  node 

ros  ::  Publisher  nav.pub  =  n  .  advertise  <MONTe:  :  Nav.Data  >(”Nav_Data_T”  ,  1); 

ros  ::  Publisher  cmd.flg.pub  =  n  .  advertise  <MONTe:  :  Command_Flags  >(”Command_Flags_T”  ,  1) 

ros  ::  Rate  loop  .rate  (4) ;  //  Sets  4hz  cycle  for  main  loop 

MONTe ::  Nav.Data  n.data; 

MONTe :  :  Command.Flags  flags; 

/*  End  ROS  Initializations  */ 


*/ 


*/ 


//  Buffer  for  nav  data  incoming  from  Monkey 


/* 

/ 

/ 
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usb_fd  =  OpenUSBSerialPort  () ; 


while  (  ros  : :  ok  ()) 

{ 

//  Receive  Navigation  data 

//  Update  Command. Flags  with  fix  quality  ( future  implementation ) 
if  (  ReadfromUSB SerialPort  (  nav.buffer  ,  254)  >  0) 

{ 

#ifdef  NAV.PRINT 


#endif 


printf  (”\nStep  1”); 


//  Parse  string  data 
Parse _Monkey  _Data  ( ) ; 


#ifdef  NAV_PRINT 


#endif 


Print_Nav_Data  (); 


#ifdef  NAV_PRINT 


#endif 


printf  (”\nStep  10\n”); 


/*  Update  navigation  data  for  publishing  */ 

n.data  .  latitude  =  GPS_Buffer  .  latitude  ; 
n.data  .  longitude  =  GPS_Buffer  .  longitude  ; 

//  n .data  .  heading  =  GP S .Buff er  .  heading  ; 

/*  Publish  navigation  data  to  topic  */ 

nav_pub  .  publish  (  n_data  ) ; 

}  //  end  if 

loop _rate  .  sleep  () ;  //  Sleeps  to  maintain  loop.rate 

}  //  end  while  loop 

CloseUSBSerialPort  (); 
return  0; 

}  //end  main 

Function:  Parse  .Monkey  .Data 

Description:  Tokenizcs  buffer  to  populate  navigation  data.  Add  or  subtract 

steps  to  adjust  what  gets  pulled  from  string  . 


Parameter :  None 


Return  Value:  0—  Success 

int  Parse_Monkey _Data  ( ) 

{ 

static  char  *buf_ptr; 
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#ifdef 

#endif 

#ifdef 

#endif 

#ifdef 

#endif 

#ifdef 

#endif 


#ifdef 

#endif 


#ifdef 

#endif 

#ifdef 

#endif 


buf.ptr  =  &nav  .buffer  [  4  ] ;  //  Start  pointer  at  4th  element  (satillites  tracked ) 

NAV.PRINT 

p  r  i  n  t  f  ( ”\ nS  tep  2”); 

NAV  .PRINT 

p  r  i  n  t  f  ( ”\ nS  tep  3”); 

NAV.PRINT 

printf  (”\nStep  4”); 


GPS.Buffer  .  sat.track  =  atoi  (  strtok  (  buf.ptr  , 

NAV.PRINT 

p  r  i  n  t  f  ( ”\ nS  tep  5”); 

//  GPS -Buffer  .  fix -quality  =  atoi  (  strtok  (NULL, 

//  Converts  ascii  fix  quality  to  an  integer 

GPS.Buffer  .  latitude  =  atof  (  strtok  (NULL,  /  10000; 

//  Converts  ascii  latitude  to  decimal  degrees 

NAV.PRINT 

p  r  i  n  t  f  ( ”\ nS  tep  6”); 


GPS.Buffer  .  longitude  =  at  of  (  s  t  r  t  ok  (NULL,  ”,”))  /  10000; 

//  Converts  ascii  longitude  to  decimal  degrees 

// GPS -Buffer  .  heading  =  atof  (  strtok  (NULL,  /  10: 

//  Converts  ascii  heading  to  double  with  2  decimal  places 

GPS.Buffer  .  altitude  =  atoi  (  strtok  (NULL, 

//  Converts  ascii  altitude  to  integer  meters  (MSL) 

NAV.PRINT 

p  r  i  n  t  f  ( ”\ nS  tep  7”); 


GPS.Buffer.  vel.N  =  at  of  (  s  t  rt  o  k  (NULL,  ”,”))  /  10; 

//  Converts  ascii  N/S  velocity  to  double  with  2  decimal  places 

NAV.PRINT 

p  r  i  n  t  f  ( ”\ nS  tep  8”); 


GPS.Buffer  .  vel.E  =  at  o  f  (  s  t  rt  o  k  (NULL,  ”,”))  /  10; 

//  Converts  ascii  E/W  velocity  to  double  with  2  decimal  places 
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#ifdef  NAV_PRINT 


#endif 


printf  (”\nStep  9”); 


GPS.Buffer  .  vel.D  =  atof  (  strtok  (NULL,  /  10; 

//  Converts  ascii  U/D  velocity  to  double  with  2  decimal  places 

buf.ptr  =  NULL; 

return  0; 

}  //  end  callback 

Function:  Print -N  av -Data 

Description  :  Printing  function  for  troubleshooting  .  Uncomment / add  lines 

to  print  out  additional  data. 

Parameter :  None 

Return  Value:  None 

void  Print_Nav_Data  () 

{ 

pri n  tf  ( ”\n\tCommand  string  \  t%s\n”  ,  GPS.Buffer  .  command.str  ) ; 
printf  (”  Satellites  tracked  \  t%d\n”  ,  GPS.Buffer  .  sat.track  ) ; 

//  printf  (”\n\tFix  quality\t%s\n” ,  GPS -Buffer  .  fix  .quality  ); 
printf  (”\tCurrent  Latitude \  t%f\n”  ,  GPS.Buffer  .  latitude  ) ; 
pri n tf  ( ”\ tCurrent  Longitude\ t%f \n”  ,  GPS.Buffer  .  longitude  ) ; 

//  printf  (”\  tCommand  String  \  t%f\n  ” ,  GPS -Buff e  r  .  heading  ) ; 
pri n tf  ( ”\tCommand  String\ t%d\n”  ,  GPS.Buffer  .  altitude  ) ; 
printf  (”\tCommand  String  \  t%f\n”  ,  GPS  .Buffer  .  vel.N  ) ; 
pri n tf  ( ”\tCommand  String\ t%f\n”  ,  GPS.Buffer  .  vel.E  ) ; 
printf  (”\tCommand  String \  t%f\n”  ,  GPS.Buffer  .  vel.D  ) ; 

}  //  end  callback 


C.4  USB-Serial  Library 

Library  for  running  RS-232  serial  communications  over  USB  port.  Used  in  conjunction  with 
MONTe  Monkey  Driver.cpp . 

Title:  MONTe_USB_Serial_Lib  0.0 

Author:  Jason  Hickle 

Purpose:  List  of  functions  to  read  and  write  serial  data  over  a  USB— Serial 

for  use  with  MONTe  on  Linux. 
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Use:  Include  ” MONTe .USB -Serial. Lib  .  h”  in  the  header  of  each  node  needed. 

Include  source  file  as  part  of  CMakelist.txt  as  follows: 
rosbuild  .add  .executable  (  node  .name  src  /  node  .name  .  cpp  s  rc  /  MONTe  .USB  .Serial  .Lib  .  cpp  ) 

Verify  that  serial  handle  ( /  dev  /  ttyUSBO )  is  correct  for  system. 

Version  History: 

-  Version  0.0  - 

Mar  21st  ,  2011 

LT  Jason  Hickle 

Open  and  Close  USB/ Serial  port.  Reads  data  from  serial  port. 

/*  Libraries  */ 

#include  <stdio.h> 

#include  <unistd.h> 

#include  <sys  / types.  h> 

#include  <sys/stat.h> 

#include  < f  c  n  1 1  .  h> 

#include  Ctermios.  h> 

#include  <string.h> 

#include  <errno.h> 

#include  ”MONTe_USB_Serial_Lib  .  h” 


/* 

D  efi  nes 

*/ 

/* 

Global  Variable  s 

*/ 

static 

int  fd  =  0; 

static 

struct  termios  oldtio  ; 

/* 

Functions 

*/ 

Function:  OpenU  SBSerialP  ort 

Description:  Takes  port  number  and  opens  appropriate  serial  connection  . 

Will  save  old  port  data 

Parameter:  1)  sPortNumber  —  pointer  to  comm  port  ttyS(X),  ie  0  for  ttySO 

etc  . 

Return  Value:  fd  —  file  descriptor  for  port 

int  OpenUSBSerialPort  () 

{ 

char  sPortName  [64]  =  ”/ dev / ttyUSBO” ;  //  Hardcoded  until  generic  method  works 

//  make  sure  port  is  closed 
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CloseUSBSerialPort  (); 

fd  =  open  (  sPortName  ,  O.RDWR  |  CLNOCTTY  |  O.NDELAY ) ; 
if  (fd  <  0) 

{ 

printf(”open  error  %d  %s\n”  ,  err  no,  strerror(errno)); 

} 

else 

{ 

struct  termios  my.termios  ; 
tcgetattr  (fd  ,  &my .termios  ) ; 

oldtio  =  my.termios;  //  Save  port  attributes  to  restore  later 
tcflush  (fd  ,  TCIFLUSH ) ; 

my.termios  .  c.cflag  =  B115200  |  CS8  |  CREAD  |  CLOCAL  |  HUPCL; 

cfsetospeed(&my_termios  ,  B115200); 
tcsetattr  (fd  ,  TCSANOW,  &my_termios  ) ; 

}  //  end  if 

return  fd  ; 

}  //  end  K .OpenSerialPort 

Function:  CloseUSBSerialPort 

Description:  Checks  to  see  if  port  is  open  and  then  closes  it.  Returns 

port  attributes  to  original  configuration  . 

Parameter :  None 

Return  Value:  None 

void  CloseUSBSerialPort  () 

{ 

//  you  may  want  to  restore  the  saved  port  attributes 
if  (fd  >  0) 

{ 

tcsetattr  (fd  ,  TCSANOW,  &oldtio); 
close ( fd  ) ; 

}  //  end  if 

}  //  end  K .Close S erialP or t 

Function  :  WritetoU SB SerialP ort 

Description  : 

Parameter:  1) 
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Return  Value: 

int  WritetoUSB  SerialPort  (  char*  psOutput) 

{ 

int  iOut  ; 

if  (fd  <  1)  //  Port  is  not  open,  return  —1 

{ 

return  — 1; 

}  //  end  if 

iOut  =  write(fd,  psOutput,  1);  //  Set  to  1  so  only  one  byte  is  xmitted 

if  ( iOut  <  0) 

{  //  Place  in  ROSINFO  statement  !!!!!! ! 

//  print/  ("  write  error  %d  %s\n” .  errno  ,  strerror  (  errno  )  ) ; 

} 

return  iOut  ; 

}  //  end  K .WritetoSerialP ort 

Function  :  ReadfromU SBSerialP ort 
Description  : 

Parameter:  1) 

Return  Value: 

int  ReadfromUSBSerialPort  ( char*  psResponse  ,  int  iMax) 

{ 

int  iln  ; 

//  printf(”  in  ReadAdrPort  iMax=%d\n” ,  iMax); 

if  (fd  <  1) 

{ 

printf(”  port  is  not  open\n”); 
return  —1; 

}  //  end  if 

strncpy  (psResponse,  ”N/A”  ,  iMax<4?iMax  :  4 ) ; 
iln  =  read(fd,  psResponse,  iMax  — 1); 

if  (iln  <  0) 

{ 

if  (errno  ==  EAGAIN) 

{ 

return  0;  //  assume  that  command  generated  no  response 

} 

else 
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{ 


printf(”read  error  %d  %s\n”  ,  err  no,  strerror(errno)); 
}  //  end  if 

} 

else 

{ 

psResponse  [  iln  <iMax?  iln  :  iMax]  =  ’\0’; 

//  p  rintf(  ”  read  %d  chars:  %s\n”,  iln,  psResponse ); 

}  //  end  if 

return  iln  ; 

}  //  end  ReadAdrP ort 


Title:  MONT e .USB .Serial. Lib  0.0  (ROSNode) 

Author:  Jason  Hickle 

Purpose:  Header  file  for  MONTe  serial  comms . 

/*  Libraries  *  / 

# include  <ros/ros.h> 

int  OpenUSBSerialPort  ( ) ; 

void  CloseUSB  SerialPort  ( ) ; 

int  WritetoUSB  SerialPort  (  char*  psOutput); 

int  ReadfromUSBSerialPort  ( char*  psResponse,  int  iMax); 


C.5  Plant  Control  Driver 

Following  driver  interfaces  with  Sabertooth  2x12  Motor  Drivers. 

Title:  MONTe -Plant -Control  0.0. 4. 2  ( ROSNode ) 

Author:  Jason  Hickle 

Purpose:  Driver  node  that  allows  MONTe  to  control  the  whegs  and  water  jets. 

Handles  the  following  functions : 

1)  Manual  Control 

2)  Communicate  with  Sabertooth2x  1 2  motor  drivers 

Use:  Receives  command  data  from  the  command  topics  and  executes. 

Commands  sent  to  Sabe rtooth2x  1 2  motor  driver  via  RS—232  interface  , 
over  port  /dev /tty  SO.  Can  run  both  whegs  and  water  jets  over  same 
interface  as  long  as  commands  have  correct  address  in  packetized 
serial  mode . 

For  plant  control  running  in  Simplified  Serial  mode  for  Sabertooth 
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2x12  motor  drivers:  Each  plant  command  will  send  4  bytes  of  data. 

1st  byte:  Left  motor  command  (1  —  full  rev,  64—  stop  ,  127— full  fwd). 

2nd  Byte:  Left  motor  command  (128  — full  rev,  190— stop,  256— full  fwd). 


For  plant  control  running  in  Packetizcd  Serial  mode  for  Sabertooth 
2x12  motor  drivers:  Each  plant  command  will  send  4  bytes  of  data. 

1st  byte:  motor  controler  address. 

2nd  Byte:  command  (fwd,  rev,  left  turn,  right  turn) 

3rd  Byte:  speed  (0  —  127) 

4th  Byte:  checksum  ,  ( address+command+speed )  &  ObOlllllll 

Manual  control  receives  desired  speed  and  turn  rate  from  topic  ,  and 
parses  data.  Converts  signed  int8-t  to  unsigned  char  for  xmit  to 
motor  controllers. 

Runs  at  system  loop  rate  of  4  Hz. 


ROS  Notes: 

Name—  ”  MONTe -Plant-Control” 

Publications—  None 

Sub  srciptions  —  ”  Plant -Commands  _T” 

Messages—  Plant -Command .  msg 

Services  —  None 

Version  History: 

-  Version  0.0. 4. 2  - 

Apr  26th  ,  2011 
LT  Jason  Hickle 

—  Enable  simplified  serial  control  for  Sabertooth2xl  2  .  Packetizcd  serial  is  problematic 

-  Version  0.0.4. 1  - 

Apr  26th  ,  2011 
LT  Jason  Hickle 


—  Enable  packetized  serial  control  for  Sabertooth2x  1 2  .  Using  alternate  logic 
for  parsing  data  commands.  Instituted  a  lhz  spin  rate  for  sending  commands. 
Using  new  message  (v2)  for  sending  command  data.  Moderate  success,  sends  right 
commands  for  a  command  or  two,  then  abberent  behavior  occurs. 

-  Version  0.0.4  - 

Apr  26th  ,  2011 
LT  Jason  Hickle 


—  Enable  packetized  serial  control  for  Sabertooth2x  1 2  .  Non— deterministic  behavior . 
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Version  0.0.3 


Apr  26th  ,  2011 
LT  Jason  Hickle 

—  Enabled  simplified  serial  control  for  Sabertooth2xl 2  .  Successful 

-  Version  0.0.2  - 

Mar  31st  ,  2011 
LT  Jason  Hickle 

—  Sue c e s s fully  receives  commands  from  control  topic  ,  and  correctly 
parses  them.  Unable  to  get  packetized  serial  to  work  with  Sabertooth2x  1 2 
motor  controllers  .  Will  persue  simplified  serial  and  come  back  to 
packetized  serial  later. 

-  Version  0.0.1  - 

Mar  31st  ,  2011 
LT  Jason  Hickle 

—  Successfully  receives  commands  from  control  topic  ,  and  correctly 
parses  them. 

-  Version  0.0  - 

Mar  21st  ,  2011 
LT  Jason  Hickle 

Enable  Manual  Control  mode  in  main  control  loop. 

Notes:  Further  information  can  be  found  on  ROS  Wiki  page: 

http  : / fwww .  ros  .  org / wiki / 

/*  Libraries  *  / 

#include  <stdio.h> 

#include  < s  t  d  1  i  b  .  h> 

#include  <unistd.h> 

#include  <sys/ types. h> 

#include  <sys/stat.h> 

#include  < f c n 1 1  . h> 

#include  Ctermios.  h> 

#include  <string.h> 

#include  <errno.h> 

#include  <stdint.h> 

#include  <unistd.h> 

#include  <ros/ros.h> 

#include  ”  std_msgs  /  S  tring  .  h” 

#include  ”MONTe/ Plant_Command  .  h”  //  Message  format  (.msg) 

#include  ”MONTe_Serial_Lib  .  h” 

#include  <sstream> 

/*  Debugging  */ 
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//  Debugging  options  ,  uncomment  to 

enable 

# define  MOTOR_CONTROL_DEBUG 

// 

Prints 

out 

commands  from 

manual  cmd 

topic 

//# define  FLOW-CHECK 

// 

Prints 

out 

stages  of  code 

,  uncomment 

to  check  that 

// 

indivi  a 

lual 

sections  are  running 

/*  D  efi  nes  *  / 

const  char  MOTORPORTJSfUMBER  =  0;  //  For  adding  ttyS(X)  selection  later 

/*  Global  Variables  */ 


//  Control  Variable s 

u  i  n  1 8  _t 

MONTe_Left ,  MONTe.Right ; 

// 

u  i  n  1 8  _t 

temp.left  =  0; 

// 

u  i  n  1 8  _t 

temp.right  =  0; 

// 

Received  data  from  Manual  .Commands. T 
temporary  storage  values  to  check  for 
if  manual  commands  have  changed 


/*  Functional  Prototypes  */ 

void  Plant_CommandCallback  (  const  MONTe :  :  Plant_CommandConstPtr&  command); 
int  Manual_Control_Simplified_Parser  (); 

int  Plant_Control_Simplified  ( unsigned  char*  s_speed  ,  const  char*  port.num); 

int  main(int  argc  ,  char  **argv) 

{ 

/*  Variable  s  */ 

//  Flags 

bool  Manual_Command_F  =  1; 

//bool  New  .Command  .F  =  1; 

/*  Initializations  *  / 

/ /  Perfo rm  initializations  for  ROS 

ros  :  :  i  n i  t  (  argc  ,  argv  ,  ” MONTe_Plant_Control”  ) ;  //  Set  up  ROS  node 

ros  : :  NodeHandle  n;  //  set  up  handle  for  this  node 

//  Set  up  all  subscriptions  for  node 

ros  ::  Subscriber  man.cmd  =  n  .  subscribe  ( ” Plant_Command_T”  ,  100,  Plant.CommandCallback  ) 

ros  ::  Rate  loop  .rate  (4) ;  //  Sets  4hz  cycle  for  main  loop 

MONTe:  :  Plant.Command  command; 

//  Initialize  variables 
MONTe.Left  =  0; 

MONTe.Right  =  0; 

char  portno  =  (char)  MOTORJPORT .NUMBER ; 

K_OpenSerialPort(&portno  ); 

/*  Main  control  loop.  * 

*  Performs  the  following  :  —  Poll  all  topics  * 
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—  Manual  control 

—  Maintains  desired  loop  rate 


*/ 

while  (  ros  : :  ok  ( ) ) 

{ 

ros  : :  spinOnce  () ;  //  Callback  to  all  active  topics 

#ifdef  MOTOR_CONTROL_DEBUG 

ROS_INFO(”  Topic  data:  Left  Speed  %d\tRight  Speed  %d”  ,  MONTe.Left,  MONTe.Right) 

#endif 


if  ( Manual.Command.F) 

{ 


} 


Manual_Control_Simplified_Parser(); 


loop _rate  .  sleep  () ;  //  Sleeps  to  maintain  loop. rate 

}  //  end  while  loop 

K.CloseSerialPort  (); 

}  // end  main 


Function  :  Plant. CommandCallback 


Description:  Pulls  manual  commands  from  the  Plant  .Command  topic 

Parameter:  1)  Pointer  to  ROS  message  from  topic  Plant  .Command 

Return  Value:  None 

void  Plant_CommandCallback  (  const  MONTe :  :  Plant_CommandConstPtr&  command) 

{ 

MONTe.Left  =  command— >  1  e  f  t  ; 

MONTe.Right  =  command— >ri  g h t  ; 

}  //  end  callback 


Function  :  Manual  .Control  .Simplified  _P  arser 


Description:  Pulls  manual  control  data  for  forward  /  reverse  speed  and 

turning  rate.  Calls  Plant  .Control  to  send  commands  to  Sabertooth 
2x12  motor  drivers. 


Parameter : 

Void 

Return  Value: 

0 

No  new  command 

1 

New  command  sent 

int  Manual.Control.Simplified.Parser  () 
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{ 


unsigned  char  sabertooth_left_speed  ;  //  Desired  left  speed. 

unsigned  char  sabertooth_right_speed  ;  //  Desired  right  speed 

/*  Check  if  new  command  is  received .  Return  0  if  not.  */ 

if  ((MONTe.Left  ==  temp_left)  &&  (MONTe.Right  ==  temp.right)) 

{ 

return  0; 

} 

#ifdef  MOTOR.CONTROL.DEBUG 

ROS_INFO( ” Current  :  Left-%d,  Right-%d\ nOrdered  :  Left-%d,  Right-%d.”, 
temp_left  ,  temp_right  ,  MONTe_Left ,  MONTe_Right ) ; 

#endif 

sabertooth_left_speed  =  (unsigned  char)  MONTe_Left; 
sabertooth  _right_speed  =  (unsigned  char)  MONTe_Right ; 

/*  Send  command  to  left  motor.  */ 

Plant_Control_Simplified(&sabertooth_left_speed  ,  <^OTOR_PORT_NUMBER ) ; 

/*  Send  command  to  right  motor.  */ 

Plant_Control_Simplified(&sabertooth_right_speed  ,  &MOTOR _PORT_NUMBER ) 

temp_left  =  MONTe_Left; 
temp.right  =  MONTe.Right; 

return  1; 

}  //  end  Manual-Command  ^Parser 


Function:  Plant -Control 

Description:  Takes  a  command  string  and  transmits  to  Sabertooth2x  1 2 

motor  drivers  via  RS  —  232.  Opens  port,  writes  address /command 
/  data  /  checksum  ,  then  closes  port.  This  is  for  simplified 
serial  mode . 


Parameter:  1)  Pointer  to  speed  command 

2)  Port  number  of  serial  eg  0  of  "tty SO” 

Return  Value:  0  Command  sent  successfully 

—1  Port  failed  to  open 

—2  Write  failed 

int  Plant.Control.Simplified  ( unsigned  char*  s.speed  ,  const  char*  port.num) 
{ 

char  port  ; 

port  =  (char)  *port_num; 


//  if  (  K -OpenSerialP ort(&port )  <0) 
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//  return  —1: 


if  (  K_WritetoSerialPort(  s_speed)<0) 

return  —2; 

//  K_Clo seSerialP ort  (  ); 

return  0; 

} 


C.6  Serial  Library 

Library  for  running  RS-232  serial  communications  over  USB  port.  Used  in  conjunction  with 
MONTe  .Plant  .Control .  cpp . 

Title:  MONTe  .Serial .Lib  0.0 

Author:  Jason  Hickle 

Purpose:  List  of  functions  to  write  serial  data  for  use  with  MONTe  on  Linux 

Use:  Include  ”  MONTe  Serial -Lib  .  h”  in  the  header  of  each  node  needed. 

Include  source  file  as  part  of  CMakelist.txt  as  follows: 
rosbuild. add. executable  (  node  .name  src  /  node  .name  .  cpp  src  /  MONTe  .Serial. Lib  .  cpp  ) 

Verify  that  serial  handle  (/dev/ttySO)  is  correct  for  system. 

Version  History: 

-  Version  0.0  - 

Mar  21st  ,  2011 
LT  Jason  Hickle 

Open  and  Close  Serial  port.  Write  data  to  serial  port. 

/*  Libraries  *  / 

# include  <stdio.h> 

#include  <unistd.h> 

#  i  n  elude  <sys  / types.  h> 

#include  <sys/stat.h> 

#  i  n  elude  <  f  c  n  1 1  .  h> 

#include  <termios.  h> 

# i n elude  <string.h> 

#include  <errno.h> 

#include  ”MONTe_Serial_Lib  .  h” 

/*  D  efi  nes  *  / 


91 


/*  Global  Variables  */ 

static  int  fd  =  0; 

static  struct  termios  oldtio  ; 

/*  Functions  */ 

Function:  K  .OpenSerialP  ort 

Description:  Takes  port  number  and  opens  appropriate  serial  connection  . 

Will  save  old  port  data 

Parameter :  1)  sPortNumber  —  pointer  to  comm  port  ttyS(X),  ie  0  for  ttySO 

etc  . 

Return  Value:  fd  —  file  descriptor  for  port 

int  K_OpenSerialPort  ( char*  sPortNumber) 

{ 

char  sPortName  [64]  =  ”  /  dev  /  tty  SO  ”  ;  //  Hardcoded  until  generic  method  works 

//  sprintf  (  sPortName  ,  ”/dev /  tty  S%c  ” ,  *  sPortNumber ) ;  //  Not  working  ,  need  to  research 

//  make  sure  port  is  closed 
K.CloseSerialPort  (); 

fd  =  open  ( sPortName  ,  O.RDWR  |  OJSfOCTTY  |  0_NDELAY ) ; 
if  (fd  <  0) 

{ 

printf(”open  error  %d  %s\n”,  err  no,  strerror(errno)); 

} 

else 

{ 

struct  termios  my_termios  ; 
tcgetattr  (fd  ,  &my .termios  ) ; 

oldtio  =  my.termios  ;  //  Save  port  attributes  to  restore  later 

tcflush  (fd  ,  TCIFLUSH ) ; 

my.termios  .  c _cf lag  =  B9600  |  CS8  |  CREAD  |  CLOCAL  |  HUPCL; 

cfsetospeed(&my_termios  ,  B9600); 
tcsetattr  (fd  ,  TCSANOW,  &my_termios  ) ; 

}  //  end  if 

return  fd  ; 

}  //  end  K -OpenSerialP ort 

Function:  K.CloseSerialPort 
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Description:  Checks  to  see  if  port  is  open  and  then  closes  it.  Returns 

port  attributes  to  original  configuration  . 

Parameter :  None 

Return  Value:  None 

void  K.CloseS erialPort  () 

{ 

//  you  may  want  to  restore  the  saved  port  attributes 
if  (fd  >  0) 

{ 

tcsetattr  (fd  ,  TCSANOW,  &oldtio); 
close ( fd  ) ; 

}  //  end  if 

}  //  end  K -Close S erialP or t 

Function  :  K .Writ etoS erialP ort 

Description  : 

Parameter:  1) 

Return  Value: 

int  K.WritetoSerialPort  ( unsigned  char*  psOutput) 

{ 

int  iOut  ; 

if  (fd  <  1)  //  Port  is  not  open,  return  —1 

{ 

return  —1; 

}  //  end  if 

iOut  =  write(fd,  psOutput,  1);  //  Set  to  1  so  only  one  byte  is  xmitted 

if  ( iOut  <  0) 

{  //  Place  in  ROS.INFO  statement  !!!!!! ! 

// printf  (”  write  error  %d  %s\n”,  errno  ,  strerror  (  errno  ) ) ; 

} 

return  iOut  ; 

}  //  end  K.WritetoSerialPort 

Function  :  K.ReadfromSerialPort 
Description  : 

Parameter:  1) 
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Return  Value: 


/*  int  K.ReadfromSerialPort  (  int8  .t*  psResponse  ,  int  iMax) 

{ 

i nt  iln  ; 

printf(”in  ReadAdrPort  iMax=%d\n”,  iMax); 

if  (fd  <  1) 

{ 

printf(”  port  is  not  open\n”); 
return  —1; 

}  //  end  if 

strncpy  ( psResponse ,  ”N/A” ,  iMax<4?iMax  :  4  ) ; 
iln  =  read(fd,  psResponse  ,  iMax  —  1); 
if  (  iln  <  0) 

{ 

if  ( errno  --  EAGAIN) 

{ 

return  0;  //  assume  that  command  generated  no  response 

} 

else 

{ 

p  r  int f  (”  read  error  %d  %s\n”,  errno,  strerror  (  errno  )) ; 

}  //  end  if 

} 

else 

{ 

psResponse  [  iln  <iMax?  iln  :  iMax  ]  =  ’\0’; 

printf(”read  %d  chars:  %s\n” ,  iln,  psResponse ); 

}  //  end  if 

return  iln; 

}  //  end  ReadAdrPort 
*/ 


Title:  MONT e Serial _Lib  0.0 

Author:  Jason  Hickle 

Purpose:  List  of  functions  to  write  serial  data  for  use  with  MONTe  on  Linux 

Use:  Include  ”  MONTe  .Serial  .Lib  .h”  in  the  header  of  each  node  needed. 

Include  source  file  as  part  of  CMakelist .  txt  as  follows: 
rosbuild  .add  .executable  (  node  .name  src  /  node  .name  .  cpp  src  /  MONTe  .Serial  .Lib  .  cpp ) 

Verify  that  serial  handle  (/dev/ttySO)  is  correct  for  system. 

Version  History: 

-  Version  0.0  - 
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Mar  21st  ,  2011 
LT  Jason  Hickle 


Open  and  Close  Serial  port.  Write  data  to  serial  port. 


/* 

Libraries 

*/ 

#include 

<  s  t  d i o . h> 

#include 

<  u  n  i  s  t  d  .  h> 

#include 

<sys/ types . h> 

#include 

<sys/ stat . h> 

#include 

<  f  c  n  1 1  .  h> 

#include 

Ctermios  .  h> 

#include 

<  s  t  r  i  n  g  .  h> 

#include 

<errno  .  h> 

#include 

”MONTe_Serial_Lib  .h” 

/* 

D  efi  nes 

*/ 

/* 

Global  Variables 

*/ 

static  int  fd  =  0; 

static  struct  termios  oldtio  ; 

/* 

Functions 

*/ 

Function:  K  .OpenSerialP  ort 

Description:  Takes  port  number  and  opens  appropriate  serial  connection  . 

Will  save  old  port  data 

Parameter:  1)  sPortNumber  —  pointer  to  comm  port  ttyS(X),  ie  0  for  ttySO 

etc  . 

Return  Value:  fd  —  file  descriptor  for  port 

int  K_OpenSerialPort  ( char*  sPortNumber) 

{ 

char  sPortName  [64]  =  ”  /  dev  /  tty  SO  ”  ;  //  Hardcoded  until  generic  method  works 

//  sprintf  ( sPortName  ,  "/dev  /  tty  S%c  ” ,  *  sPortNumber ) ;  //  Not  working,  need  to  research 

//  make  sure  port  is  closed 
K.CloseSerialPort  (); 

fd  =  open  ( sPortName  ,  OJRDWR  |  0_NOCTTY  |  0_NDELAY ) ; 
if  (fd  <  0) 

{ 

printf(”open  error  %d  %s\n”,  err  no,  strerror(errno)); 

} 

else 

{ 

struct  termios  my.termios  ; 
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tcgetattr  (fd  ,  &my _termios  ) ; 


oldtio  =  my_termios  ;  //  Save  port  attributes  to  restore  later 

tcflush  (fd  ,  TCIFLUSH ) ; 

my.termios  .  c _cf lag  =  B9600  |  CS8  |  CREAD  |  CLOCAL  |  HUPCL; 

cfsetospeed(&my_termios  ,  B9600); 
tcsetattr  (fd  ,  TCSANOW,  &my_termios  ) ; 

}  //  end  if 

return  fd  ; 

}  //  end  K -OpenSerialP ort 

Function:  K -CloseSerialP  ort 

Description:  Checks  to  see  if  port  is  open  and  then  closes  it.  Returns 

port  attributes  to  original  configuration  . 

Parameter :  None 

Return  Value:  None 

void  K.CloseSerialPort  () 

{ 

//  you  may  want  to  restore  the  saved  port  attributes 
if  (fd  >  0) 

{ 

tcsetattr  (fd  ,  TCSANOW,  &oldtio); 
close ( fd  ) ; 

}  //  end  if 

}  //  end  K -Close S erialP or t 

Function  :  K -Write to S erialP ort 

Description  : 

Parameter:  1) 

Return  Value: 

int  K_WritetoSerialPort  ( unsigned  char*  psOutput) 

{ 

int  iOut  ; 

if  (fd  <  1)  //  Port  is  not  open,  return  —1 

{ 

return  —  1; 

}  //  end  if 
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iOut  =  write(fd,  psOutput  ,  1); 


//  Set  to  1  so  only  one  byte  is  xmitted 


if  ( iOut  <  0) 

{  //  Place  in  ROSJNFO  statement  !!!!!! ! 

//  printf  (”  write  error  %d  %s\n”,  errno  ,  str  error  (  errno  )) ; 

} 

return  iOut  ; 

}  //  end  K.WritetoSerialPort 

Function  :  K_ReadfromSerialPort 
Description  : 

Parameter:  1) 

Return  Value: 

/*  int  K  .ReadfromSerialP  ort  (  int  8  _t  *  psResponse  ,  int  iMax) 

{ 

i  nt  iln  ; 

printf  (”  in  ReadAdrPort  iMax=%d\n”,  iMax); 

>f  (fd  <  1) 

{ 

printf ("  port  is  not  open\n" ); 
return  — 1 ; 

}  //  end  if 

strncpy  (psResponse,  ”N/A” ,  iMax<4?iMax  :  4  ) ; 
iln  =  read(fd  ,  psResponse  ,  iMax  —  1); 
if  (iln  <  0) 

{ 

if  ( errno  ==  EAGAIN) 

{ 

return  0;  //  assume  that  command  generated  no  response 

} 

else 

{ 

printf  (”  read  error  %d  %s\n” ,  errno,  strerror  (  errno  )) ; 

}  //  end  if 

} 

else 

{ 

psResponse  [  iln  <iMax?  iln  :  iMax ]  =  ’\0 

printf  (’’read  %d  chars:  %s\n”,  iln,  psResponse ); 

}  //  end  if 

return  iln; 

}  //  end  ReadAdrPort 
*/ 
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C.7  Keyboard  Control  Node 

Following  code  performs  manual  control  of  MONTe. 


Title:  MONTe  .Keyboard  .Control  0.5  (ROS  Node ) 

Author:  Jason  Hickle 

Purpose:  Node  that  allows  MONTe  to  be  controlled  via  keyboard  over  a  VNC 

server . 

Handles  the  following  functions : 

1)  Take  keyboard  commands 

2)  Change  the  speed  and  turning  rates 

3)  Publishes  manual  commands  to  ROS  topic 

4)  Updates  command  flags  to  manual  control  upon  command 

Use:  Use  the  arrow  keys  to  move.  P  stops  the  robot.  U  prompts  user  to 

change  speed.  I  prompts  user  to  change  turn  rate.  Works  with 
simplified  serial  mode . 


ROS  Notes: 

Name- 

”  MONTe  .Keyboard.  Control  ” 

Publications— 

”  Plant  .Commands  _T” 

”  Command. Flags  _T” 

Subscriptions  — 

None 

Messages  — 

Plant  .Command .  msg 

Command.Flags .  msg 

Services  — 

None 

Version  History 


-  Version  0.5 

— 

Apr  26th,  2011 

LT  Jason  Hickle 

Added  publishing  to  ” Command. Flags _T”  to  update  when  receiving  manual  commands 

-  Version  0.4  - 

Apr  26th  ,  2011 
LT  Jason  Hickle 

Using  simplified  serial  command  for  motors  and  utilizing  Plant. Command 
msg  format.  Allow  user  to  change  fwd/rev  speed  and  turning  rate. 
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-  Version  0.3  - 

Apr  26th  ,  2011 
LT  Jason  Hickle 

Using  packetized  serial  command  for  motors  and  utilizing  Manual  .Command  _2 
msg  format.  Allow  user  to  change  speed  and  turning  rate. 

-  Version  0.2  - 

Apr  10th  ,  2011 
LT  Jason  Hickle 

Changed  commands  to  simplified  serial  format  from  prior  packetized 
serial  format.  Sends  new  msg  format  to  ”  Manual -Commands -T  ” .  Return  to 
Manual-Command  .msg  vice  Manual -Command -Simplified .  msg  if  running  in 
packetized  serial  mode . 

-  Version  0.1  - 

Mar  31st  ,  2011 
LT  Jason  Hickle 

Added  debug  options  to  verify  correct  commands.  Fixed  incorrect  keycodes . 

-  Version  0.0  - 

Mar  31st  ,  2011 
LT  Jason  Hickle 

Enable  keyboard  commands  to  send  manual  control  signals  over  to 
topic  Manual -Commands _T  for  packetized  serial  mode. 

Notes:  Further  information  can  be  found  on  ROS  Wiki  page: 

http  :  / /www .  ros  .  org  /  wiki  / 

/*  Libraries  */ 

#include  <ros/ros.h> 

#include  < s i g n a  1  .  h> 

#include  Ctermios.  h> 

#include  <stdio.h> 

#include  < s  t  d  1  i  b  .  h> 

# include  ”MONTe/ Plant.Command  .  h” 

#include  ”MONTe/ Command.Flags  .  h” 

/*  Debugging  */ 

# define  C0MMAND_VALUE_PR1NT 


/* 

D  efi  nes 

#define 

KEY  CODE  _RIGHT 

0x43 

#define 

KF.YCODF,  T  F.FT 

0x44 

#define 

KEYCODE_UP 

0x41 

#define 

KEYCODEXIOWN 

0x42 

#define 

KEYCODE_Q 

0x71 
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# define  KEY CODE_SPACE  0x20 


#define  KEYCODE.U  0x75 

#define  KEYCODEJ  0x69 

#define  KEYCODE.Y  0x79 

#define  KEYCODEJ  0x6a 

#define  KEYCODEJ!  0x68 

const  unsigned  char  STOP_R  =  190; 

const  unsigned  char  STOP.L  =  64; 

/*  Global  Variables  */ 

//  Keyboard  control  variables 
int  kfd  =  0; 

struct  termios  cooked,  raw; 

/ /  Command  variables 

unsigned  char  fwd_spd  =  30;  //  Value  for  going  forward 

unsigned  char  rev.spd  =  20;  //  Value  for  going  reverse 

unsigned  char  turn.spd  =  10;  //  Differential  turning  speed 

unsigned  char  temp_fwd_spd  =  30;  //  temporary  storage  variable s 

unsigned  char  temp_rev_spd  =  20; 
unsigned  char  temp_turn_spd  =  20; 

unsigned  char  tempj_turn  =  64;  //  Temp  vars  for  computing  turning  rates. 

unsigned  char  temp.r.turn  =  190; 
unsigned  char  temp.turn  =  10; 

unsigned  char  *fwd_spd_ptr  ,  *rev_spd_ptr  ,  *  turn.spd.ptr  ,  *  temp  J_turn_ptr  ,  *  temp _r .turn _ptr  ; 

//  Pointers  for  code  optimization 

unsigned  char  L.Correction  =  3;  //  Calibration  coefficients 

unsigned  char  R.Correction  =  1; 

unsigned  char  temp  J.correct  ,  temp _r_correct  ; 

unsigned  char  *  l.correct.ptr  ,  * r _c orre c t _p tr ; 

unsigned  char  turn.flag  =  0; 

/*  Functional  Prototypes  */ 

void  quit(int  sig); 

unsigned  char  turn.simplified  ( unsigned  char  speed,  char  flag); 

int  main(int  argc  ,  char  **argv) 

{ 

/ /  Initializations 

/ /  Perfo rm  initializations  for  ROS 

ros  :  :  init(argc,  argv  ,  ’’MONTe.Keyboard.Control”  ) ;  //  Set  up  ROS  node 

ros  :  :  NodeHandle  n;  //  set  up  handle  for  this  node 
//  Set  up  all  publications  for  node 

ros  ::  Publisher  man.cmd.pub  =  n  .  advertise  <MONTe:  :  Plant.Command  >(”Plant_Command_T”  ,  1); 
ros  ::  Publisher  cmd.flg.pub  =  n  .  advertise  <MONTe:  :  Command  Jdags  >(”Command_Flags_T”  ,  1); 

MONTe : :  Plant.Command  cmd ; 


100 


MONTe ::  Command-Flags  flags; 

/ /  End  ROS  initialization 

/*  Variable  and  pointer  initialization  */ 

cmd .  left  =  STOP.L ; 
cmd.  right  =  STOP_R ; 
fwd_spd-ptr  =  &fwd_spd  ; 
rev_spd_ptr  =  &rev_spd  ; 
turn_spd_ptr  =  &turn_spd ; 
temp_l_turn_ptr  =  &temp-l_turn  ; 
temp _r .turn _ptr  =  &temp_r_turn  ; 

1  _c o rre c t _p tr  =  &L_Correction  ; 
r_correct_ptr  =  &R_Correction  ; 

//  Set  the  auto_nav  flag  to  0  (manual)  when  manual  command  is 
flags  .  auto_nav  =  0; 

signal  ( SIGINT  ,  quit  ) ; 

//  Initialize  keyboard 

//  get  the  console  in  raw  mode 
tegetattr  (kfd  ,  &cooked); 

memcpy(&raw  ,  &cooked  ,  sizeof  (  struct  termios)); 
raw  .  c  _lf  1  ag  &=~  (ICANON  |  ECHO); 

//  Setting  a  new  line  ,  then  end  of  file 
raw  .  c_cc  [VEOL]  =  1; 
raw  .  c_cc  [VEOF]  =  2; 
tesetattr  (kfd  ,  TCSANOW,  &raw  ) ; 


std 

:  cout 

« 

’’Reading  from  keyboard\n” ; 

std 

:  cout 

« 

— 

\n  ’ ; 

std 

:  cout 

« 

"Use 

arrow  keys 

to  move  MONTe. \n”; 

std 

:  cout 

« 

”P 

stops  MONTe. \ 

n”  ; 

std 

:  cout 

« 

”Y 

to 

enter  new 

reverse  speed. \n”; 

std 

:  cout 

« 

”U 

to 

enter  new 

reverse  speed. \n”; 

std 

:  cout 

« 

”1 

to 

enter  new 

turn  rate . \ n”  ; 

std 

:  cout 

« 

”H 

to 

enter  new 

left  wheg  correction  .\n”  ; 

std 

:  cout 

« 

to 

enter  new 

right  wheg  correction  .\n”  ; 

std 

:  cout 

« 

’’Current  settings:  Fwd  speed  ”  «  (int)  fwd.spd  « 

«  (int)  rev.spd  «  ”  Turn  rate  ”  «  (int)  turn.spd  « 
//puts(”Q  quits  ROS.”); 


while  (  ros  :  :  ok  ( ) ) 

{ 

char  c ; 

bool  dirty  =  false ; 

//  get  the  next  event  from  the  keyboard 
if(read(kfd,  &c ,  1)  <  0) 

{ 

perror  ( ” read  ():”); 


received 


”  Rev  speed 

\n”; 
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exi t  (  —  1 ); 

} 

ROS_DEBUG(  ’’value  :  0x%02X\n”  ,  c  ) ; 

switch  (c ) 

{ 

case  KF.YCODF,  LEFT :  //  Turn  left 
ROS_DEBUG(  ’’LEFT”  ) ; 

//  Computes  turn  differential  speed  for  whegs  and  adds  correction  factor 

*  temp_l  _turn_ptr  =  turn.simplified  (STOP_L  +  fwd.spd  ,  0); 

*  te mp _r _turn _p tr  =  turn_simplified  (STOP_R  +  fwd_spd  ,  1); 

cmd  .left  =  *temp_l_turn_ptr; 

cmd.  right  =  *temp_r_turn_ptr; 

dirty  =  true  ; 

ROS_INFO(” Going  Left\n”); 

break  ; 

case  KEYCODE_RIGHT :  //  Turn  right 
ROS_DEBUG(  ’’RIGHT”  ) ; 

//  Computes  turn  differential  speed  for  whegs  and  adds  correction  factor 

*  temp.l  _turn_ptr  =  turn_simplified  (STOP.L  +  fwd.spd,  1); 

*  te mp  _r  .turn  _p tr  =  turn.simplified  (STOP.R  +  fwd.spd  ,  0); 

cmd  .left  =  *temp_l_turn_ptr; 

cmd.  right  -  *temp_r_turn_ptr; 

dirty  =  true  ; 

ROS_INFO(” Going  Right\n”); 

break  ; 

case  KEYCODE.UP:  //  Go  forward 

ROS .DEBUG ( ”UP”  ) ; 

//  Enters  desired  fwd  speed  and  adds  calibration  correction 
cmd.  left  =  STOP.L  +  *fwd_spd_ptr  +  *  1  .correct.ptr  ; 
cmd.  right  =  STOP.R  +  *fwd_spd_ptr  +  *  r.correct.ptr  ; 

dirty  =  true  ; 

ROS_INFO( ’’Going  Forward\n”); 

break  ; 

case  KEYCODE.DOWN :  //  Go  in  reverse 

ROS  .DEBUG  ( ’’DOWN”  ) ; 

//  Enters  desired  rev  speed  and  applies  calibration  correction 
cmd.  left  =  STOP.L  —  *rev_spd_ptr  —  *l_correct_ptr; 
cmd.  right  =  STOP.R  —  *rev_spd_ptr  —  *r_correct_ptr; 

dirty  =  true  ; 
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ROS_INFO( ’’Going  Backwards\n”  ) ; 


break  ; 

case  KEYCODE.SPACE:  //  Stop 

ROS_DEBUG(  ’’STOP” ) ; 

cmd .  left  =  STOP.L; 
cmd.  right  -  STOP_R; 

dirty  =  true  ; 

ROS_INFO(”Stopping\n”  ); 

break  ; 

case  KEYCODE_Y :  //  Enter  new  forward  speed 

ROS_DEBUG( ” Enter  new  forward  speed”); 

do  {  //  User  inputs  speed  and  checks  for  valid  input 

std  :  :  cout  «  ”\nEnter  forward  speed  (0—50).”; 
scanf(”%d”,  &temp_fwd_spd  ) ; 

}  while  ( temp_fwd_spd  >  50); 

fwd.spd  =  temp_fwd_spd ; 

dirty  =  true  ; 

ROS_INFO( ’’New  forward  speed  %d\n”  ,  fwd.spd); 
continue  ;  //  Return  to  top  of  while  loop 

case  KEYCODE_U:  //  Enter  new  reverse  speed 

ROS_DEBUG( ” Enter  new  reverse  speed”); 

do  {  //  User  inputs  speed  and  checks  for  valid  input 

printf  (”\nEnter  new  reverse  speed  (0—50).”); 
scanf(”%d”,  &temp_rev_spd); 

}  while  ( temp_rev_spd  >  50); 

rev.spd  =  temp_rev_spd  ; 

dirty  =  true  ; 

ROS_INFO( ’’New  reverse  speed  %d\n”  ,  rev.spd); 
continue  ;  //  Return  to  top  of  while  loop 

case  KEYCODEJ:  //  Enter  new  turn  rate 
ROS_DEBUG( ” Enter  turn  rate”); 

do  {  //  User  inputs  speed  and  checks  for  valid  input 

printf  (”\nEnter  turn  rate  (0—40).”); 
scanf(”%d”,  &temp_turn_spd  ) ; 

}  while  ( temp_turn_spd  >  40); 
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turn.spd  =  temp  .turn  _spd  ; 


dirty  =  true  ; 

ROS_INFO( ’’New  turn  rate  %d\n”  ,  turn.spd); 
continue  ;  //  return  to  top  of  while  loop 

case  KEYCODE_H:  //  Enter  correction  factor  for  left  set  of  whegs 
ROS_DEBUG(  ”  Enter  left  speed  correction”); 

do  {  //  User  inputs  speed  and  checks  for  valid  input 

printf(”\nEnter  left  correction  factor  (0  —  13).”); 
scanf(”%d”,  &temp_l_correct); 

}  while  ( temp _1  .correct  >  13); 

*  1  _correct_ptr  =  temp _1  .correct  ; 
dirty  =  true  ; 

ROS_INFO( ’’New  left  correction  %d\n”  ,  *  1  _correct_ptr  ) ; 
continue  ;  //  return  to  top  of  while  loop 

case  KEYCODEJ :  //  Enter  correction  factor  for  right  set  of  whegs 
ROS_DEBUG(  ”  Enter  left  speed  correction”); 

do  {//  User  inputs  speed  and  checks  for  valid  input 

printf(”\nEnter  right  correction  factor  (0  —  13).”); 
scanf(”%d”,  &temp_r_correct); 

}  while  ( temp _r .correct  >  13); 

*  r  .correct  _ptr  =  temp _r .correct  ; 
dirty  =  true  ; 

ROS_INFO( ’’New  left  correction  %d\n”  ,  *  r.correct.ptr  ) ; 
continue  ;  //  return  to  top  of  while  loop 


default  : 

continue;  //  return  to  top  of  while  loop 

}  //  end  switch 

#ifdef  COMMAND_VALUE_PRINT 

ROS_INFO(  ”  Speed  :  Left  %d\tRight  %d”  ,  cmd.left  ,  cmd.  right); 

#endif 


man.cmd.pub  .  publish  (cmd ) ; 
cmd.flg.pub  .  publish(flags  ); 

dirty  =  false  ; 

}  //  End  main  while  loop 
}  //end  main 
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Function:  quit 


Description  : 

Parameter:  1) 

Return  Value: 

void  quit(int  sig) 

{ 

tcsetattr  (kfd  ,  TCSANOW,  &cooked); 
ros  :  :  shutdown  ( ) ; 
exit  ( 0 ) ; 

} 


Function:  turn  .simplified 

Description:  Takes  turn  signals  and  outputs  proper  simplified  serial  value 

Parameter:  1)  Forward  speed 

2)  Inside  whegs  (0),  outside  whegs  (1) 

Return  Value:  Simplified  serial  value  (64  —  127  for  left  whegs,  190—255  for  right 

whegs  ). 

unsigned  char  turn_simplified  ( unsigned  char  speed,  char  flag) 

{ 

unsigned  char  final_speed; 
if  (  speed  <  64) 

return  0;  //  Bad  forward  turn  command,  sends  a  0  which  will 

//  stop  the  motor 

if ( speed  >=  190) 

{  if  (flag  ==  0)//  For  the  right  motor,  inside  track,  ensures  min  turn  speed  is  190  (stop) 

final.speed  =  ((speed  —  *turn_spd_ptr  +  *r_correct_ptr)  >=  190)  ? 

(speed  —  * turn_spd_ptr  +  * r _correct_ptr )  :  190; 

else  if  (flag  ==  1)//  If  right  side  it  outside  turn,  max  turn  spd  at  255  (full  fwd) 

final.speed  =  ((speed  +  *turn_spd_ptr  +  *r_correct_ptr)  <=  255)  ? 

(speed  +  * turn_spd_ptr  +  * r _correct_ptr )  :  255; 

}  else  //  Left  motor  command,  min  turn  speed  is  64(  stop )  and  127  (full  fwd) 

{  if  (flag  ==  0)//  For  the  right  motor,  inside  track,  ensures  min  turn  speed  is  64  (stop) 

final_speed  =  ((speed  —  *  turn_spd_ptr  +  *  l_correct_ptr  )  >=  64)  ? 

(speed  —  * turn_spd_ptr  +  * l_correct_ptr )  :  64; 
else  if  (flag  ==  1)//  If  right  side  it  outside  turn,  max  turn  spd  at  127  (full  fwd) 

final_speed  =  ((speed  +  *  turn_spd_ptr  +  *  1  _c  o  r  r  e  c  t  _p  tr  )  <=  127)  ? 

(speed  +  * turn_spd_ptr  +  * l_correct_ptr )  :  127; 

}  //  end  parsing  if 
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return  final_speed; 
}  //  end  turn. simplified 


C.8  Waypoint  Control  Node 


Following  code  performs  manual  control  of  MONTe. 


Title:  MONTe  .Waypoint  .Control  0.0  (ROS  Node ) 

Author:  Jason  Hickle 


Purpose:  Node  that  allows  user  to  input  waypoints  over  VNC  server. 

Handles  the  following  functions : 

1)  Add  waypoints  to  queue. 

2)  Send  route  to  New  .Waypoints 

3)  Delete  all  waypoints 

4)  Update  Command. Flags  when  in  auto—nav. 

Use:  Rudimentary  user  input  program.  Inputs  waypoints  and  then  sends 

them  to  Waypoint  .P  roce  s  sing  via  N  ew. Way  point. T .  Route  is  sent  at  4Hz 
to  ensure  waypoints  are  not  lost.  This  synchs  up  with  program  rate. 

Utilizes  class  structure  for  manipulating  route. 

Actions  not  used  in  current  code,  but  kept  for  future  use.  Could  be 
used  to  indicate  search  routines  ,  return  to  base,  etc.  Current 
behavior  is  to  stop  after  completion  of  last  waypoint . 

Next  step  is  to  take  the  class  structure  further  and  subsume 
ROS  initilizations  into  the  private  portion.  This  will  tranisition 
to  a  more  object-oriented  format  than  currently  implemented . 


ROS  Notes: 

Name—  ”  MONTe  .Waypoint.  Control 

Publications—  ”  New  .Waypoints  .T  ” 

”  Command. Flags  .T” 


Subscriptions—  None 


Messages—  Waypoint,  msg 

Command. Flags .  msg 


Services  —  None 


Version  History: 

-  Version  0.0  - 

May  19th,  2011 
LT  Jason  Hickle 
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Allow  input  ,  deletion  and  sending  of  new  waypoints . 


Notes:  Further  information  can  be  found  on  ROS  Wiki  page: 

http  :  / /www .  ros  .  org  /  wiki  / 

/*  Libraries  */ 

#include  <ros/ros.h> 

#include  < s i g n a  1  .  h> 

#include  Ctermios.  h> 

#include  <stdio.h> 

#include  <stdlib.h> 

#include  ”MONTe/ Command_Flags  .  h”  //  MONTe  messages 

#include  ’’MONTe/  Waypoint .  h” 

/*  D  efi  nes  */ 

//  Debugging  options  ,  uncomment  to  enable 

#define  WP_PRINT  //  Print  statement  for  new  waypoints 

#define  KEYCODEJ)  0x64  //  For  user  inputs 

#define  KEYCODEJM  0x6e 

#define  KEYCODE.S  0x73 

/*  WP  stores  all  data  necessary  to  navigate  to,  and  determine  / 

/  behavior  mode  for  a  route.  */ 

typedef  struct  { 

int  number; 
double  latitude  ; 
double  longitude  ; 
char  action  ; 

}  WP; 

/*  class  Waypoints  provides  methods  for  manipulating  a  waypoint  route  */ 

class  Waypoints  { 
private  : 

public  : 

struct  { 

int  number; 
double  latitude  ; 
double  longitude  ; 
char  action  ; 

}  wp  [10]; 

int  wp_count ; 
bool  wp_entered  ; 

Waypoints(){  //  Constructor  for  class  Waypoints 

wp[0].  number  -  0; 
wp.count  =  0; 
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wp_entered  =  0;  //  Ensures  route  not  sent  until  wp  is  entered 

} 


void  Add.Waypoint  ( ) ; 
void  Delete.Route  ( ) ; 

}; 

void  Waypoints  ::  Add.Waypoint  () 

{ 

double  temp.lat  ,  temp.lon  ; 
char  temp.action  ; 

wp [  wp_count  ].  number  =  wp_count ; 

printf  (”\nPlease  enter  waypoint  %d  latitude  (dec  degrees,  N  is  positive): 
scanf(”%lf”,  &temp_lat); 

wp[  wp_count  ] .  1  ati  tu  de  =  temp_lat; 

printf  (”\nPlease  enter  waypoint  %d  longitude  (dec  degrees,  E  is  positive): 
scanf(”%lf”,  &temp_lon); 

wp[  wp.count  ].  longitude  =  temp.lon; 

//  Need  to  get  better  i/o  option  for  following 
/*  //do  { 

printf  (”\nPlease  enter  waypoint  %d  action  (a—  continue  ,  s—stop):  ”, 
scanf(”%c”,  &temp  .action  ) ; 

//}  while  ((  temp  .action  !=  ’a’)  ||  (temp. action  !=  ’s  ’)); 

wp  [  wp  .count  ].  action  =  temp. action;  */ 


#ifdef  WP_PRINT 


ROS.INFO ( ”Wp  #%d,  Latitude  -  %lf  ,  Longitude  =  %lf  ”  , 

wp[  wp  .count],  longitude); 


#endif 


wp.count  ++; 
wp.entered  =  1; 


wp.count,  wp[  wp.count  ].  latitude 


void  Waypoints  ::  Delete.Route  ()  //  Recursive  function  to  delete  stored  wp  data 

{ 

if  (wp.count  >  9)  //  Prevents  out  of  out  of  bounds 

wp.count  =  9; 

while  ( wp.count  >  0)  //  Deletes  all  data  until  wp.count=0 

{ 

wp[  wp.count  ].  number  =  0;  //  Null  all  data 

wp[  wp.count  ].  latitude  =  0.0; 
wp[  wp.count  ].  longitude  =  0.0; 
wp[  wp.count  ].  action  =  NULL; 


,  wp.count); 


,  wp.count) 


wp  .count ) ; 
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wp_count - ;  //  Decrement  wp -count  for  next  recursion 

Waypoints  ::  Delete.Route  () ;  //  Call  Delete-Route  again  to  delete  next  wp 


} 


} 

if(wp_count  ==  0)  //  Check  in  case  of  bad  data 

{ 

wp[  wp.count  ].  number  =  0;  //  Null  all  data 

wp[  wp.count  ].  latitude  =  0.0; 
wp[  wp.count  ].  longitude  =  0.0; 
wp[  wp.count  ].  action  =  NULL; 

wp_entered  =  0;  //  Reset  flag  to  indicate  no  stored  waypoints 


/*  Global  Variables  */ 

//  Keyboard  control  variables 
int  kfd  =  0; 

struct  termios  cooked,  raw; 
int  counter  ; 

/*  Functional  Prototypes  */ 

void  Instructions  () ; 

void  quit(int  sig); 

int  main(int  argc  ,  char  **argv) 

{ 

/*  Initializations  *  / 

/ /  Perfo rm  initializations  for  ROS 

ros  :  :  init(argc,  argv  ,  ”MONTe_Waypoint_Control”  ) ;  //  Set  up  ROS  node 

ros  :  :  NodeHandle  n;  //  set  up  handle  for  this  node 

ros::  Rate  loop  .rate  (4) ;  //  Sets  4hz  cycle  for  main  loop 

//  Set  up  all  publications  for  node 

ros  ::  Publisher  cmd_flg_pub  =  n  .  advertise  <MONTe:  :  Command.Flags >(”Command_Flags_T”  ,  1) 

ros  ::  Publisher  new_wp_pub  =  n  .  ad  verti  se  <MONTe ::  Waypoint  >(”New_Waypoint_T”  ,  10); 

MONTe :  :  Command_Flags  flags; 

MONTe ::  Waypoint  new.wp  ; 

signal  ( SIGINT  ,  quit  ) ; 

Waypoints  Waypoint_Queue  ; 

/*  Initialize  keyboard  for  user  input  */ 

//  get  the  console  in  raw  mode 
tegetattr  (kfd  ,  &cooked); 

memcpy(&raw  ,  &cooked  ,  sizeof  (  struct  termios)); 
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raw .  c  .lflag  &='  (ICANON  |  ECHO); 


//  Setting  a  new  line  ,  then  end  of  file 
raw  .  c_cc  [VEOL]  =  1; 
raw  .  c_cc  [VEOF]  =  2; 
t  c  s  e  t  a  1 1  r  ( kfd  ,  TCSANOW,  &raw  ) ; 
while  (  ros  : :  ok  ( )) 

{ 

char  input.cmd  ; 

Instructions  () ;  //  Print  instructions  at  each  loop 

//  get  the  next  event  from  the  keyboard 
if(read(kfd,  &input_cmd  ,  1)  <  0) 

{ 

perror  ( ” read  ():”); 
exit  ( —  1); 

} 


switch  (input.cmd) 

{ 

case  KEYCODEJSf:  //  Enter  new  waypoint 

if  (  Waypoint.Queue  .  wp.count  <  10) 

Way  point  .Queue  .  Add  .Waypoint  ( ) ; 
else 


break ; 


ROS.INFO ( "Max  waypoints  reached!”); 


case  KEYCODE.S:  //  Send  new  route 

if  ((  Waypoint.Queue  .  wp.entered  ==  1)  &&  ( Waypoint.Queue  .  wp.count  >  0)) 

{ 

counter  =  0;  //  Send  waypoints 


//  Warn  Waypoint -Processing  new  route  incoming 
flags  .  auto.nav  =  0; 
f  1  ag s  .  nav.mode  =  ’A’; 

//  Navigation  takes  over  nav .mode 
flags  .  incoming.route  =  1; 

//  Indicate  new  route  to  system 
cmd_flg_pub  .  publish(flags  ); 

1  o op  .rate  .  sleep  () ;  //  Sleeps  to  maintain  loop.rate 

//  Enter  send  waypoint  loop,  will  send  messages  until  all  wp  sent 
while  (  counter  <  Waypoint.Queue  .  wp.count ) 

{ 

new.wp  .  latitude  =  Waypoint.Queue  .wp[  counter  ].  latitude  ; 

new.wp  .  longitude  =  Waypoint.Queue  .wp[  counter  ].  longitude 

new.wp .  acti  on  =  ’a’;  //  stop  and  wait 

new.wp  .  wp.num  =  counter; 

new.wp.  route  =  Waypoint.Queue  .  wp.count ; 


no 


new_wp_pub  .  publish  (new_wp  ) ; 

//  Publish  to  ROS  Topic 

#ifdef  WP.PRINT 

ROS_INFO( ’’Waypoint  %d  sent!”,  counter); 
#endif 

flags  .  auto.nav  =  1; 

//Place  MONTe  in  autonomous  navigation 
if(counter  ==  0) 

f  1  ag s  .  nav.mode  =  ’N’ ; 

//Tell  Waypoint  .Processing  to  send  first  waypoint 
//  for  first  waypoint  sent 

else 

f  1  ag  s  .  nav_mode  =  ’A’; 
//Navigation  takes  over  nav.mode 
flags  .  incoming_route  =  1; 

//Indicate  new  route  to  system 
cmd_flg_pub  .  publish(flags  ); 
counter  ++; 
loop.rate  .  sleep  (); 

//  Sleeps  to  maintain  loop  .rate 
}  //  End  send  waypoint  loop 
}  else 

{ 

ROS_INFO(”No  route  in  queue  to  send.”); 

continue  ; 

} 


ROS_INFO( ’’MONTe  is  in  autonomous  navigation”); 

break  ; 

case  KEYCODE_D:  //  Delete  route 

i f  ( Waypoint_Queue  .  wp.entered  -=  1) 

Way  point -Queue.  Delete_Route  (); 
else 

ROS_INFO(”No  waypoints  in  queue  to  delete.” 


break  ; 


default  : 

continue  ; 

}  //  end  switch 

}  //  end  main  while  loop 

}  //end  main 

Function :  Instructions 


ill 


Description  : 


Print  out  instructions  for  node 


Parameter :  None 

Return  Value:  None 

void  Instructions  () 

{ 

printf  (”\nWelcome  to  MONTe  waypoint  control  ...\n”); 

printf  (”  Please  enter  :  \  n\  t  \  tn  —  Enter  new  waypoint\n\  t  \  td  —  Delete  route\n”); 
printf (”\ t\ ts  —  Send  route\n”); 

}  //  End  Instructions 

Function:  quit 

Description  : 

Parameter:  1) 

Return  Value: 

void  quit(int  sig) 

{ 

tcsetattr  (kfd  ,  TCSANOW,  &cooked); 
ros  : :  shutdown  ( ) ; 
exit  ( 0 ) ; 
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APPENDIX  D: 
ROS  Messages 


The  following  are  the  message  formats  used  by  MONTe. 

#  All  messages  for  MONTe 

#  Used  for  sending  information  between  topics 

#  Command-Flags  .  msg 

#  Contains  all  behavior  flags  for  MONTe. 

#  Autonomous  Navigation  flag  :  1  for  autonav  ,  0  for  manual  control 
bool  auto.nav 

#  Navigation  Mode:  A  —  Enroute  to  current  Waypoint,  N—  Current  waypoint  reached 
char  nav_mode 

#  Route  flag  indicates  new  set  of  waypoints 
bool  incoming-route 

#  Nav.Data  .  msg 

#  Send  a  waypoint  for  MONTe. 

#  Latitude  in  decimal  degrees  . 
float64  latitude 

#  Longitude  in  decimal  minutes 
float64  longitude 

#  Heading 
float64  heading 

#  Action  character 
char  action 

#  Plant-Command  .  msg 

# 

#  Basic  message  for  manually  controlling  MONTe  in  simplified  serial  mode. 

#  Speed  command  for  left  motor.  Range  is  l(Full  Reverse)— >  64  (Stop)  <—  127  (Full  Forward) 
uint8  left 

#  Speed  command  for  left  motor.  Range  is  128(Full  Reverse)— >  192  (Stop)  <—  255  (Full  Forward) 
uint8  right 
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#  Waypoint .  msg 

# 

#  Send  a  waypoint  for  MONTe. 

#  Latitude  in  decimal  degrees  . 
float64  latitude 

#  Longitude  in  decimal  minutes 
float64  longitude 

#  Action  character 
char  action 

#  Waypoint  number  (0  —  9) 
int8  wp_num 

#  Number  of  waypoints  in  route  ( New_Waypoint  only) 
int8  route 
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